diff --git a/.circleci/config.yml b/.circleci/config.yml index c587587da8..e32bc967bd 100644 --- a/.circleci/config.yml +++ b/.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 diff --git a/.gitmodules b/.gitmodules index 3220cb00fe..b984c04a7b 100644 --- a/.gitmodules +++ b/.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 diff --git a/perception/realsense_driver/python/realsense_wrapper/realsense_d435.py b/perception/realsense_driver/python/realsense_wrapper/realsense_d435.py index 21acfc1fcf..3741627a0b 100644 --- a/perception/realsense_driver/python/realsense_wrapper/realsense_d435.py +++ b/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() diff --git a/perception/sandbox/eyehandcal/calibration.json b/perception/sandbox/eyehandcal/calibration.json new file mode 100644 index 0000000000..4013492a6f --- /dev/null +++ b/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 + ] + } +] \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_fre.json b/perception/sandbox/eyehandcal/calibration_fre.json new file mode 100644 index 0000000000..a1e6140522 --- /dev/null +++ b/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 + ] + } + +] \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_grasp.json b/perception/sandbox/eyehandcal/calibration_grasp.json new file mode 100644 index 0000000000..fc2f56882f --- /dev/null +++ b/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 + ] + } +] \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_place.json b/perception/sandbox/eyehandcal/calibration_place.json new file mode 100644 index 0000000000..d9a399943a --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_place.json @@ -0,0 +1,52 @@ +[ + { + "num_marker_seen": 0, + "stage2_retry": null, + "pixel_error": null, + "proj_func": null + }, + { + "num_marker_seen": 20, + "stage2_retry": 1, + "pixel_error": 0.909844828313588, + "proj_func": "hand_marker_proj_world_camera", + "camera_base_ori": [ + [ + -0.7895167178559055, + 0.006719481144470554, + 0.61369226881165 + ], + [ + -0.04660491999337455, + 0.9963964203831648, + -0.07086716362339691 + ], + [ + -0.611956970430481, + -0.08455188951622115, + -0.7863584706231546 + ] + ], + "camera_base_ori_rotvec": [ + -0.027675187976721628, + 2.478681221327078, + -0.10784014504885431 + ], + "camera_base_pos": [ + -0.633655247867228, + -0.5765724258636483, + 0.21029529819389187 + ], + "p_marker_ee": [ + 0.020000548807465247, + 0.0750085816981591, + 0.0692226266791234 + ] + }, + { + "num_marker_seen": 0, + "stage2_retry": null, + "pixel_error": null, + "proj_func": null + } +] \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points copy 2.json b/perception/sandbox/eyehandcal/calibration_points copy 2.json new file mode 100644 index 0000000000..e69de29bb2 diff --git a/perception/sandbox/eyehandcal/calibration_points copy 3.json b/perception/sandbox/eyehandcal/calibration_points copy 3.json new file mode 100644 index 0000000000..45afd0d6c7 --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points copy 3.json @@ -0,0 +1 @@ +{"xyz": [[0.5265178084373474, -0.433401882648468, 0.105398801863193512], [0.2255553901195526, -0.5832930207252502, 0.11903288215398788], [0.5186301469802856, -0.1844359189271927, 0.10349027812480927], [0.19679592549800873, -0.22447608411312103, 0.07638932764530182]], "quat": [[0.7380295991897583, 0.13181553781032562, 0.65980464220047, -0.050939254462718964], [0.7904497981071472, -0.11441032588481903, 0.5259089469909668, 0.29243654012680054], [0.7231845855712891, 0.22335276007652283, 0.6312503218650818, -0.1692354530096054], [0.7898196578025818, 0.30155566334724426, 0.5060029029846191, -0.17090965807437897], [0.8611174821853638, 0.10176808387041092, -0.48675650358200073, 0.10577370226383209], [0.6848399043083191, 0.726000189781189, 0.061574045568704605, -0.011255414225161076], [0.6848363280296326, 0.7260037064552307, 0.06157286837697029, -0.011251312680542469], [0.9770357012748718, 0.19849886000156403, 0.055740922689437866, -0.05378060042858124], [0.9516550898551941, -0.29526132345199585, 0.041929882019758224, -0.07358816266059875], [0.9119481444358826, -0.1920102834701538, 0.3626042902469635, -0.0008977177203632891], [0.7931819558143616, 0.3259752690792084, 0.47685280442237854, -0.1929091513156891]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points copy 4.json b/perception/sandbox/eyehandcal/calibration_points copy 4.json new file mode 100644 index 0000000000..c21984631c --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points copy 4.json @@ -0,0 +1 @@ +{"xyz": [[0.28748682141304016, -0.4995553493499756, 0.11579205095767975], [0.6663658618927002, -0.37118086218833923, 0.1340467631816864], [0.6641161441802979, -0.05277084559202194, 0.1350872814655304], [0.29183292388916016, -0.11817368865013123, 0.11988697946071625]], "quat": [[0.8099788427352905, 0.39686915278434753, -0.34379637241363525, 0.2612147629261017], [0.7629833817481995, -0.028823232278227806, -0.6343039870262146, 0.12117766588926315], [0.5584104061126709, 0.48399022221565247, 0.5278392434120178, -0.4187087118625641], [0.6313112378120422, 0.3518662750720978, 0.6397061944007874, -0.26155734062194824], [0.8818773627281189, 0.46778371930122375, 0.054258108139038086, 0.02295099012553692], [0.7272232174873352, 0.6847040057182312, 0.04091804474592209, 0.025544146075844765]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points copy.json b/perception/sandbox/eyehandcal/calibration_points copy.json new file mode 100644 index 0000000000..ae082f5022 --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points copy.json @@ -0,0 +1 @@ +{"xyz": [[0.2462332546710968, -0.13003762066364288, 0.18732747435569763], [0.2293388992547989, -0.5544804930686951, 0.21611009538173676], [0.6373224258422852, -0.5196283459663391, 0.21525612473487854], [0.6929877996444702, -0.024927478283643723, 0.17892567813396454]], "quat": [[0.9938040971755981, 0.037247732281684875, 0.08467312157154083, -0.06161535903811455], [0.6338156461715698, 0.7730087637901306, -0.004607343580573797, 0.026720095425844193], [0.872911810874939, 0.4846484065055847, 0.018690776079893112, 0.05283519998192787], [0.7893615961074829, 0.09432769566774368, 0.6064858436584473, -0.01361742615699768], [0.6569705605506897, 0.6160807609558105, 0.3178236782550812, 0.29634830355644226], [-0.5148396492004395, -0.5072444081306458, 0.5413282513618469, -0.4296590983867645]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points.json b/perception/sandbox/eyehandcal/calibration_points.json index 61c2e1cff3..3776ecfc8e 100644 --- a/perception/sandbox/eyehandcal/calibration_points.json +++ b/perception/sandbox/eyehandcal/calibration_points.json @@ -1 +1 @@ -{"xyz": [[0.7841147184371948, -0.26561641693115234, 0.237627312541008], [0.8307967782020569, 0.02608540467917919, 0.2515111565589905], [0.8506162166595459, -0.02456057071685791, 0.347420334815979], [0.8517714738845825, -0.05609707906842232, 0.4044360816478729], [0.8047099709510803, -0.2907401919364929, 0.3745661973953247], [0.8047105073928833, -0.29073861241340637, 0.37456685304641724]], "quat": [[0.8225067853927612, -0.08389443904161453, 0.4012543261051178, -0.39425790309906006], [0.6798139810562134, -0.3220716714859009, 0.6247647404670715, -0.20926488935947418], [0.5894655585289001, -0.43789276480674744, 0.6651686429977417, -0.13539202511310577]]} \ No newline at end of file +{"xyz": [[0.20748682141304016, -0.7095553493499756, 0.1579205095767975], [0.7063658618927002, -0.7118086218833923, 0.1340467631816864], [0.7041161441802979, -0.05277084559202194, 0.1350872814655304], [0.29183292388916016, -0.05817368865013123, 0.21988697946071625]], "quat": [[0.6866209506988525, 0.7255187034606934, -0.04238300397992134, 0.019440947100520134], [0.8715306520462036, 0.4889504909515381, -0.020461317151784897, 0.030710194259881973], [0.9965093731880188, 0.07935018837451935, -0.00585085479542613, 0.025266043841838837], [0.7292221188545227, 0.56758713722229, 0.18183544278144836, 0.33617833256721497], [0.8984895348548889, 0.19719721376895905, -0.13841503858566284, 0.3669756352901459]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points_grasp.json b/perception/sandbox/eyehandcal/calibration_points_grasp.json new file mode 100644 index 0000000000..ae082f5022 --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points_grasp.json @@ -0,0 +1 @@ +{"xyz": [[0.2462332546710968, -0.13003762066364288, 0.18732747435569763], [0.2293388992547989, -0.5544804930686951, 0.21611009538173676], [0.6373224258422852, -0.5196283459663391, 0.21525612473487854], [0.6929877996444702, -0.024927478283643723, 0.17892567813396454]], "quat": [[0.9938040971755981, 0.037247732281684875, 0.08467312157154083, -0.06161535903811455], [0.6338156461715698, 0.7730087637901306, -0.004607343580573797, 0.026720095425844193], [0.872911810874939, 0.4846484065055847, 0.018690776079893112, 0.05283519998192787], [0.7893615961074829, 0.09432769566774368, 0.6064858436584473, -0.01361742615699768], [0.6569705605506897, 0.6160807609558105, 0.3178236782550812, 0.29634830355644226], [-0.5148396492004395, -0.5072444081306458, 0.5413282513618469, -0.4296590983867645]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points_grasp_lower_height.json b/perception/sandbox/eyehandcal/calibration_points_grasp_lower_height.json new file mode 100644 index 0000000000..67a69b60bf --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points_grasp_lower_height.json @@ -0,0 +1 @@ +{"xyz": [[0.2462332546710968, -0.13003762066364288, 0.15], [0.2293388992547989, -0.5544804930686951, 0.15], [0.6373224258422852, -0.5196283459663391, 0.15], [0.6929877996444702, -0.024927478283643723, 0.15]], "quat": [[0.9938040971755981, 0.037247732281684875, 0.08467312157154083, -0.06161535903811455], [0.6338156461715698, 0.7730087637901306, -0.004607343580573797, 0.026720095425844193], [0.872911810874939, 0.4846484065055847, 0.018690776079893112, 0.05283519998192787], [0.7893615961074829, 0.09432769566774368, 0.6064858436584473, -0.01361742615699768], [0.6569705605506897, 0.6160807609558105, 0.3178236782550812, 0.29634830355644226], [-0.5148396492004395, -0.5072444081306458, 0.5413282513618469, -0.4296590983867645]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points_jul20.json b/perception/sandbox/eyehandcal/calibration_points_jul20.json new file mode 100644 index 0000000000..cf15b9edf6 --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points_jul20.json @@ -0,0 +1 @@ +{"xyz": [[0.20748682141304016, -0.7095553493499756, 0.21579205095767975], [0.7063658618927002, -0.7118086218833923, 0.2340467631816864], [0.7041161441802979, -0.05277084559202194, 0.1350872814655304], [0.29183292388916016, -0.05817368865013123, 0.11988697946071625]], "quat": [[0.8099788427352905, 0.39686915278434753, -0.34379637241363525, 0.2612147629261017], [0.7629833817481995, -0.028823232278227806, -0.6343039870262146, 0.12117766588926315], [0.5584104061126709, 0.48399022221565247, 0.5278392434120178, -0.4187087118625641], [0.6313112378120422, 0.3518662750720978, 0.6397061944007874, -0.26155734062194824], [0.8818773627281189, 0.46778371930122375, 0.054258108139038086, 0.02295099012553692], [0.7272232174873352, 0.6847040057182312, 0.04091804474592209, 0.025544146075844765]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_points_place.json b/perception/sandbox/eyehandcal/calibration_points_place.json new file mode 100644 index 0000000000..1a4cd3ea84 --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_points_place.json @@ -0,0 +1 @@ +{"xyz": [[-0.05953561142086983, -0.4670037031173706, 0.044725265353918076], [0.015987128019332886, -0.6555245518684387, 0.10549433529376984], [-0.28470009565353394, -0.49809229373931885, 0.0406365729868412], [-0.21538473665714264, -0.6972994208335876, 0.1041293814778328]], "quat": [[0.6944868564605713, -0.5987726449966431, 0.1477135717868805, 0.3705941438674927], [0.8764191269874573, -0.38052600622177124, -0.09529461711645126, 0.27929991483688354], [0.6573432087898254, -0.5329223871231079, 0.4492402970790863, 0.28649041056632996], [-0.6223832368850708, 0.663404643535614, -0.18546944856643677, -0.37166446447372437]]} \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/calibration_with_original_points.json b/perception/sandbox/eyehandcal/calibration_with_original_points.json new file mode 100644 index 0000000000..52f00366ae --- /dev/null +++ b/perception/sandbox/eyehandcal/calibration_with_original_points.json @@ -0,0 +1,116 @@ +[ + { + "num_marker_seen": 154, + "stage2_retry": 1, + "pixel_error": 1.2866968835720198, + "proj_func": "hand_marker_proj_world_camera", + "camera_base_ori": [ + [ + 0.998297265717574, + -0.05016061710139268, + -0.0297738434338549 + ], + [ + -0.01313908982732666, + -0.6906735926296813, + 0.7230472686916937 + ], + [ + -0.056832504601905996, + -0.7214249101160953, + -0.6901566238794343 + ] + ], + "camera_base_ori_rotvec": [ + -2.3328613510346785, + 0.04370046427740098, + 0.05979076052903139 + ], + "camera_base_pos": [ + 0.5171904053024029, + -0.8729530913848408, + 0.4459292855933256 + ], + "p_marker_ee": [ + 0.02055231369214212, + 0.07528174580707003, + 0.06672817410557466 + ] + }, + { + "num_marker_seen": 11, + "stage2_retry": 1, + "pixel_error": 0.43305306712185915, + "proj_func": "hand_marker_proj_world_camera", + "camera_base_ori": [ + [ + 0.8331252955082901, + -0.3004621142361294, + 0.46435413198654785 + ], + [ + -0.5511752171232505, + -0.5207287655480323, + 0.6519566187714952 + ], + [ + 0.04591428986016138, + -0.7991020401923513, + -0.5994395777282804 + ] + ], + "camera_base_ori_rotvec": [ + -2.1515664090478537, + 0.6204443238492479, + -0.37174643984776923 + ], + "camera_base_pos": [ + 0.14008386215240237, + -0.8342197954235364, + 0.2831945747404522 + ], + "p_marker_ee": [ + 0.019441870829840172, + 0.07419668471659299, + 0.07225429485930936 + ] + }, + { + "num_marker_seen": 79, + "stage2_retry": 11, + "pixel_error": 2.65809931690769, + "proj_func": "hand_marker_proj_world_camera", + "camera_base_ori": [ + [ + -0.0114355680954931, + 0.7388902506632082, + -0.6737287475365696 + ], + [ + 0.9963388643634132, + -0.048668421672343934, + -0.07028692688507313 + ], + [ + -0.08472363980234864, + -0.6720659061481882, + -0.7356285221848442 + ] + ], + "camera_base_ori_rotvec": [ + -1.8354658446955474, + -1.7965046883512001, + 0.7852353662960658 + ], + "camera_base_pos": [ + 0.9534418105046142, + -0.3190106661946271, + 0.4324643931400178 + ], + "p_marker_ee": [ + 0.023507949474084532, + 0.07237089086086969, + 0.06657557599488474 + ] + } +] \ No newline at end of file diff --git a/perception/sandbox/eyehandcal/librealsense b/perception/sandbox/eyehandcal/librealsense new file mode 160000 index 0000000000..c94410a420 --- /dev/null +++ b/perception/sandbox/eyehandcal/librealsense @@ -0,0 +1 @@ +Subproject commit c94410a420b74e5fb6a414bd12215c05ddd82b69 diff --git a/perception/sandbox/eyehandcal/src/eyehandcal/scripts/collect_data_and_cal.py b/perception/sandbox/eyehandcal/src/eyehandcal/scripts/collect_data_and_cal.py index d3728ad8de..661ad0862f 100755 --- a/perception/sandbox/eyehandcal/src/eyehandcal/scripts/collect_data_and_cal.py +++ b/perception/sandbox/eyehandcal/src/eyehandcal/scripts/collect_data_and_cal.py @@ -74,14 +74,21 @@ def robot_poses(ip_address, pose_generator, time_to_go=3): ) # Get reference state + robot_stuck_count = 0 robot.go_home() for i, (pos_sampled, ori_sampled) in enumerate(pose_generator): print( f"Moving to pose ({i}): pos={pos_sampled}, quat={ori_sampled.as_quat()}") state_log = robot.move_to_ee_pose(position=pos_sampled,orientation=ori_sampled.as_quat(),time_to_go = time_to_go) print(f"Length of state_log: {len(state_log)}") + if len(state_log) < time_to_go*700: + robot_stuck_count += 1 + if robot_stuck_count > 4: + robot.go_home() + robot_stuck_count = 0 + continue if len(state_log) != time_to_go * robot.hz: - print(f"warning: log incorrect length. {len(state_log)} != {time_to_go * robot.hz}") + print(f"warning: log incorrect length. {len(state_log)} != {time_to_go * robot.hz}, robot stuck count = {robot_stuck_count}") while True: pos0, quat0 = robot.get_ee_pose() time.sleep(1) @@ -125,11 +132,11 @@ def main(argv): parser.add_argument('--ip', default='100.96.135.68', help="robot ip address") parser.add_argument('--datafile', default='caldata.pkl', help="file to either load or save camera data") parser.add_argument('--overwrite', default=False, action='store_true', help="overwrite existing datafile, if it exists") - parser.add_argument('--marker-id', default=9, type=int, help="ID of the ARTag marker in the image") + parser.add_argument('--marker-id', default=17, type=int, help="ID of the ARTag marker in the image") parser.add_argument('--calibration-file', default='calibration.json', help="file to save final calibration data") parser.add_argument('--points-file', default='calibration_points.json', help="file to load convex hull to sample points from") - parser.add_argument('--num-points', default=20, type=int, help="number of points to sample from convex hull") - parser.add_argument('--time-to-go', default=3, type=float, help="time_to_go in seconds for each movement") + parser.add_argument('--num-points', default=10, type=int, help="number of points to sample from convex hull") + parser.add_argument('--time-to-go', default=5, type=float, help="time_to_go in seconds for each movement") parser.add_argument('--imagedir', default=None, help="folder to save debug images") parser.add_argument('--pixel-tolerance', default=2.0, type=float, help="mean pixel error tolerance (stage 2)") proj_funcs = {'hand_marker_proj_world_camera' :hand_marker_proj_world_camera, diff --git a/perception/sandbox/eyehandcal/src/eyehandcal/scripts/record_calibration_points.py b/perception/sandbox/eyehandcal/src/eyehandcal/scripts/record_calibration_points.py index e9b8ab9559..538d4da8eb 100755 --- a/perception/sandbox/eyehandcal/src/eyehandcal/scripts/record_calibration_points.py +++ b/perception/sandbox/eyehandcal/src/eyehandcal/scripts/record_calibration_points.py @@ -20,9 +20,9 @@ import argparse parser = argparse.ArgumentParser() - parser.add_argument("--ip", default="100.96.135.68", help="robot ip address") - parser.add_argument("--marker-id", type=int, default=9, help="ARTag marker id") - parser.add_argument("--marker-length", type=float, default=0.05, help="ARTag length in meters") + parser.add_argument("--ip", default="100.97.47.79", help="robot ip address") + parser.add_argument("--marker-id", type=int, default=7, help="ARTag marker id") + parser.add_argument("--marker-length", type=float, default=0.04, help="ARTag length in meters") args = parser.parse_args() print(f"Config: {args}") diff --git a/perception/sandbox/polygrasp/.envrc b/perception/sandbox/polygrasp/.envrc new file mode 100644 index 0000000000..f0916362be --- /dev/null +++ b/perception/sandbox/polygrasp/.envrc @@ -0,0 +1 @@ +layout_miniconda polygrasp diff --git a/perception/sandbox/polygrasp/MANIFEST.in b/perception/sandbox/polygrasp/MANIFEST.in new file mode 100644 index 0000000000..a015a63b08 --- /dev/null +++ b/perception/sandbox/polygrasp/MANIFEST.in @@ -0,0 +1,3 @@ +global-exclude *.pyc +global-exclude __pycache__ +recursive-include src/polygrasp/*.capnp diff --git a/perception/sandbox/polygrasp/README.md b/perception/sandbox/polygrasp/README.md new file mode 100644 index 0000000000..f55bbb6148 --- /dev/null +++ b/perception/sandbox/polygrasp/README.md @@ -0,0 +1,51 @@ +# Polygrasp + +## Installation + +```bash +git submodule update --init --recursive . +mrp up --norun +``` + +Then, follow the instructions in the forked repos under [third_party](./third_party/) to download pretrained model weights: + +1. [UnseenObjectClustering](./third_party/UnseenObjectClustering/README.md) + 1. `gdown --id 1O-ymMGD_qDEtYxRU19zSv17Lgg6fSinQ -O ./third_party/UnseenObjectClustering/data/` +1. [graspnet-baseline](./third_party/graspnet-baseline/README.md) + 1. `gdown --id 1hd0G8LN6tRpi4742XOTEisbTXNZ-1jmk -O ./third_party/graspnet-baseline/data/graspnet/` + +## Usage + +Make necessary configuration modifications. For example: +- [conf/run_grasp.yaml](./conf/run_grasp.yaml) contains the configuration, e.g. robot IP. +- [conf/masks/](./conf/masks/) contains a folder to define workspace masks for each camera, for each bin. +- We expect calibrated camera parameters out of [eyehandcal](../eyehandcal); if this is not calibrated, please follow instructions there. + + +Ensure Polymetis is running on the machine connected to the robot. Then, start the necessary pointcloud/grasping/gripper servers: + +```bash +mrp up + +mrp ps # Ensure processes are alive +mrp logs --old # Check logs +``` + +The example script to bring everything together and execute the grasps: + +```bash +conda activate mrp_polygrasp +python scripts/run_grasp.py # Connect to robot, gripper, servers; run grasp +``` + +This continuously grasps from bin 1 to bin 2 until there are no more objects detected in the bin 1 workspace; then it moves the objects back from bin 2 to bin 1, and repeats. + +### Mocked data + +To test without a robot or cameras, run + +```bash +python scripts/run_grasp.py robot=robot_mock cam=cam_mock num_bin_shifts=1 num_grasps_per_bin_shift=1 +``` + +which runs the loop without connecting to a real robot and loading the RGBD images from [data/rgbd.npy](data/rgbd.npy). diff --git a/perception/sandbox/polygrasp/conf/cam/cam_mock.yaml b/perception/sandbox/polygrasp/conf/cam/cam_mock.yaml new file mode 100644 index 0000000000..a7511cc076 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/cam/cam_mock.yaml @@ -0,0 +1,5 @@ +cam: + _target_: polygrasp_test.mocked.MockedCam + rgbd_path: data/rgbd.npy + intrinsics_file: conf/intrinsics.json + extrinsics_file: ../eyehandcal/calibration.json \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/cam/cam_realsense.yaml b/perception/sandbox/polygrasp/conf/cam/cam_realsense.yaml new file mode 100644 index 0000000000..5bbaf3da34 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/cam/cam_realsense.yaml @@ -0,0 +1,4 @@ +cam: + _target_: polygrasp.cam_pub_sub.PointCloudSubscriber + intrinsics_file: conf/intrinsics.json + extrinsics_file: ../eyehandcal/calibration.json diff --git a/perception/sandbox/polygrasp/conf/evaluator.yaml b/perception/sandbox/polygrasp/conf/evaluator.yaml new file mode 100644 index 0000000000..253c6ae7d8 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/evaluator.yaml @@ -0,0 +1,137 @@ + +defaults: +# rollout +- envs: partial_visible_env # full_visible_env +# - policy: learned_policy # expert_pick_only_policy +- preference: p1 +#dataset +- trainer/dataset_partial@dataset_partial: dual_model +- trainer/padding_partial@padding: dual_model # input_pad_fn # pad_fn # prompt_pad_fn +# logging +- trainer/logger_partial@logger_partial: dual_model +# model +- trainer/transformer_model@model: dual_model +- trainer/submodule/category_encoder@category_encoder: mlp +- trainer/submodule/pose_encoder@pose_encoder: fourier_mlp +- trainer/submodule/temporal_encoder@temporal_encoder: embed +- trainer/submodule/reality_marker_encoder@reality_marker_encoder: embed +# optimization +- trainer/optimizer_partial@optimizer_partial: sgd +- trainer/scheduler_partial@scheduler_partial: exponential_lr +- trainer/criterion@criterion: dual_model +- _self_ + +rollout_savefolder: '' +to_train: true +model_name: dual_model +session_split_filepath: "session_split_filepaths/num_obj/num_pref-7_num_obj-3_num_demo-1000000" +test_split_filepath: "" +log_per_session_metrics: true +id: '' +pick_only: true +batch_size: 128 +num_targets_per_step: 64 +# init params assigned to sub-files here! +# data +# data_name: demo-pref +# data_version: 0 +# context_history: 0 +# pref: "pref_0123_456-top_first-True_back-center" +# pref: "pref_${preference.category_order_numbers_top_rack}_${preference.category_order_numbers_bottom_rack}-top_first-${preference.load_top_rack_first}_${preference.place_close_to}" +max_objects_in_rack: 3 +# dataset_path: "artifacts/${data_name}:v${data_version}/${pref}/${num_objects_per_rack}" +num_workers: 4 + +# model arch +category_embed_size: 64 +C_embed: 16 # category_embed_size // 4 +pose_embed_size: 128 +temporal_embed_size: 32 +marker_embed_size: 32 +d_model: 256 +d_hid: 512 +num_encoder_layers: 2 +num_decoder_layers: 2 +n_input_dim: 3 +num_slots: 100 +slot_iters: 1 + +# optim +lr: 0.01 +patience: 1000000000 +gamma: 0.9995 # 0.99 + +# logging +logging_interval: 100 # 100 +rollout_interval: 10000000 +max_train_evals: 2 #1 +max_val_evals: 2 #1 +max_test_evals: 500 #1 + +# misc +# num_pref: 12 +seed: 42 +device: 'cpu' +pkg_root: '' + +learner: # keep the name same for backreferences in submodules + _target_: temporal_task_planner.trainer.evaluator.Evaluator + config: + seed: ${seed} + to_train: ${to_train} + # context_history: ${context_history} + # max_epochs: 100 + max_steps: 10000000 + num_targets_per_step: ${num_targets_per_step} + max_session_data_limit: 1000000 + batch_size: ${batch_size} + patience: ${patience} + load_chkpt: true + logging_interval: ${logging_interval} + rollout_interval: ${rollout_interval} + pick_only: ${pick_only} + device: ${device} + chkpt_name: best_pickplace_TP # best_pickplace_TP_good_chkpt + # dataset_path: ${dataset_path} # "artifacts/${data_name}:v${data_version}" + # data: + # name: ${data_name} # full + # version: ${data_version} # 4 + pkg_root: ${pkg_root} + max_train_evals: ${max_train_evals} + max_val_evals: ${max_val_evals} + max_test_evals: ${max_test_evals} + padding: ${padding} + session_split_filepath: ${session_split_filepath} + test_split_filepath: ${test_split_filepath} + log_per_session_metrics: ${log_per_session_metrics} + num_workers: ${num_workers} + dataset_partial: ${dataset_partial} + model: ${model} + criterion: ${criterion} + optimizer_partial: ${optimizer_partial} + scheduler_partial: ${scheduler_partial} + logger_partial: ${logger_partial} + +wandb: + project: "num_obj" + entity: 'dishwasher_arrange' + run: '' + +hydra: + run: + dir: output/${wandb.project}/${hydra.job.override_dirname}/ + sweep: + dir: multirun/${wandb.project} + # subdir: "${session_split_filepath}/seed-${seed}" + subdir: ${session_split_filepath}/seed-${seed} + job: + config: + override_dirname: + exclude_keys: + - id + - seed + - wandb.project + - logging_interval + - saving_interval + - rollout_interval + - test_split_filepath \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/intrinsics copy.json b/perception/sandbox/polygrasp/conf/intrinsics copy.json new file mode 100644 index 0000000000..c1af68006c --- /dev/null +++ b/perception/sandbox/polygrasp/conf/intrinsics copy.json @@ -0,0 +1,47 @@ +[ + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 616.7277221679688, + "fy": 616.9970092773438, + "height": 480, + "ppx": 330.6875915527344, + "ppy": 244.88636779785156, + "width": 640 + }, + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 618.444091796875, + "fy": 617.8651123046875, + "height": 480, + "ppx": 326.2725524902344, + "ppy": 237.3859100341797, + "width": 640 + }, + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 617.420166015625, + "fy": 617.5342407226562, + "height": 480, + "ppx": 322.64678955078125, + "ppy": 244.8739776611328, + "width": 640 + } +] \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/intrinsics.json b/perception/sandbox/polygrasp/conf/intrinsics.json new file mode 100644 index 0000000000..88fad7e86a --- /dev/null +++ b/perception/sandbox/polygrasp/conf/intrinsics.json @@ -0,0 +1,32 @@ +[ + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 616.7277221679688, + "fy": 616.9970092773438, + "height": 480, + "ppx": 330.6875915527344, + "ppy": 244.88636779785156, + "width": 640 + }, + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 617.420166015625, + "fy": 617.5342407226562, + "height": 480, + "ppx": 322.64678955078125, + "ppy": 244.8739776611328, + "width": 640 + } +] \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/intrinsics_fre.json b/perception/sandbox/polygrasp/conf/intrinsics_fre.json new file mode 100644 index 0000000000..231aca6d55 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/intrinsics_fre.json @@ -0,0 +1,47 @@ +[ + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 618.4440307617188, + "fy": 617.8651123046875, + "height": 480, + "ppx": 430.2725524902344, + "ppy": 237.3859100341797, + "width": 848 + }, + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 616.7276611328125, + "fy": 616.9970092773438, + "height": 480, + "ppx": 434.6876220703125, + "ppy": 244.88636779785156, + "width": 848 + }, + { + "coeffs": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "fx": 617.4201049804688, + "fy": 617.5342407226562, + "height": 480, + "ppx": 426.64678955078125, + "ppy": 244.8739776611328, + "width": 848 + } +] \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/masks/bin1/1.png b/perception/sandbox/polygrasp/conf/masks/bin1/1.png new file mode 100644 index 0000000000..973daf964e Binary files /dev/null and b/perception/sandbox/polygrasp/conf/masks/bin1/1.png differ diff --git a/perception/sandbox/polygrasp/conf/masks/bin1/2.png b/perception/sandbox/polygrasp/conf/masks/bin1/2.png new file mode 100644 index 0000000000..973daf964e Binary files /dev/null and b/perception/sandbox/polygrasp/conf/masks/bin1/2.png differ diff --git a/perception/sandbox/polygrasp/conf/masks/bin1/3.png b/perception/sandbox/polygrasp/conf/masks/bin1/3.png new file mode 100644 index 0000000000..973daf964e Binary files /dev/null and b/perception/sandbox/polygrasp/conf/masks/bin1/3.png differ diff --git a/perception/sandbox/polygrasp/conf/masks/bin2/1.png b/perception/sandbox/polygrasp/conf/masks/bin2/1.png new file mode 100644 index 0000000000..973daf964e Binary files /dev/null and b/perception/sandbox/polygrasp/conf/masks/bin2/1.png differ diff --git a/perception/sandbox/polygrasp/conf/masks/bin2/2.png b/perception/sandbox/polygrasp/conf/masks/bin2/2.png new file mode 100644 index 0000000000..973daf964e Binary files /dev/null and b/perception/sandbox/polygrasp/conf/masks/bin2/2.png differ diff --git a/perception/sandbox/polygrasp/conf/masks/bin2/3.png b/perception/sandbox/polygrasp/conf/masks/bin2/3.png new file mode 100644 index 0000000000..973daf964e Binary files /dev/null and b/perception/sandbox/polygrasp/conf/masks/bin2/3.png differ diff --git a/perception/sandbox/polygrasp/conf/o3d_view.json b/perception/sandbox/polygrasp/conf/o3d_view.json new file mode 100644 index 0000000000..e73b6e4fa4 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/o3d_view.json @@ -0,0 +1,41 @@ +{ + "class_name" : "PinholeCameraParameters", + "extrinsic" : + [ + 0.28142335070175561, + 0.82567964910260616, + -0.48893150311428485, + 0.0, + 0.94624675005352421, + -0.15413178729053639, + 0.28435977240068283, + 0.0, + 0.15943019065703956, + -0.54267532577446542, + -0.82467296857762828, + 0.0, + -0.11735080584901347, + -0.60509490445918845, + 1.259222836495665, + 1.0 + ], + "intrinsic" : + { + "height" : 1080, + "intrinsic_matrix" : + [ + 935.30743608719376, + 0.0, + 0.0, + 0.0, + 935.30743608719376, + 0.0, + 959.5, + 539.5, + 1.0 + ], + "width" : 1920 + }, + "version_major" : 1, + "version_minor" : 0 +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/policy/expert_partial_env_pick_only_with_pref_policy.yaml b/perception/sandbox/polygrasp/conf/policy/expert_partial_env_pick_only_with_pref_policy.yaml new file mode 100644 index 0000000000..4d5c4f3447 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/policy/expert_partial_env_pick_only_with_pref_policy.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.policy.expert_policy.ExpertInteractPickOnlyWithPreferencePolicy +env: ${envs} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/policy/expert_partial_env_pick_place_with_pref_policy.yaml b/perception/sandbox/polygrasp/conf/policy/expert_partial_env_pick_place_with_pref_policy.yaml new file mode 100644 index 0000000000..7d09a6f8ee --- /dev/null +++ b/perception/sandbox/polygrasp/conf/policy/expert_partial_env_pick_place_with_pref_policy.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.policy.expert_policy.ExpertInteractPickPlacePreferencePolicy +env: ${envs} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/policy/expert_pick_only_policy.yaml b/perception/sandbox/polygrasp/conf/policy/expert_pick_only_policy.yaml new file mode 100644 index 0000000000..76b749259c --- /dev/null +++ b/perception/sandbox/polygrasp/conf/policy/expert_pick_only_policy.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.policy.expert_policy.ExpertPickOnlyPolicy +env: ${envs} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/policy/expert_pick_only_with_pref_policy.yaml b/perception/sandbox/polygrasp/conf/policy/expert_pick_only_with_pref_policy.yaml new file mode 100644 index 0000000000..9510a0326f --- /dev/null +++ b/perception/sandbox/polygrasp/conf/policy/expert_pick_only_with_pref_policy.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.policy.expert_policy.ExpertPickOnlyPreferencePolicy +env: ${envs} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/policy/expert_pick_place_with_pref_policy.yaml b/perception/sandbox/polygrasp/conf/policy/expert_pick_place_with_pref_policy.yaml new file mode 100644 index 0000000000..abe81c6f5a --- /dev/null +++ b/perception/sandbox/polygrasp/conf/policy/expert_pick_place_with_pref_policy.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.policy.expert_policy.ExpertPickPlacePreferencePolicy +env: ${envs} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/policy/learned_policy.yaml b/perception/sandbox/polygrasp/conf/policy/learned_policy.yaml new file mode 100644 index 0000000000..4c990f5037 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/policy/learned_policy.yaml @@ -0,0 +1,6 @@ +_target_: temporal_task_planner.policy.learned_policy.LearnedPolicy +chkpt_path: ${chkpt_path} +model: ${model} +context_history: ${context_history} +device: ${device} +pick_only: ${pick_only} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/robot/robot_hw.yaml b/perception/sandbox/polygrasp/conf/robot/robot_hw.yaml new file mode 100644 index 0000000000..4230746f73 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/robot/robot_hw.yaml @@ -0,0 +1,10 @@ +robot: + _target_: polygrasp.robot_interface.GraspingRobotInterface + enforce_version: false + # ip_address: "ipv6:[2620:10d:c0b9:4b05::11f3]" + ip_address: 100.97.47.82 + k_approach: 0.9 + k_grasp: 0.55 + gripper_max_width: 0.05 + gripper: + _target_: polymetis.GripperInterface diff --git a/perception/sandbox/polygrasp/conf/robot/robot_mock.yaml b/perception/sandbox/polygrasp/conf/robot/robot_mock.yaml new file mode 100644 index 0000000000..b2d1ec0def --- /dev/null +++ b/perception/sandbox/polygrasp/conf/robot/robot_mock.yaml @@ -0,0 +1,2 @@ +robot: + _target_: polygrasp_test.mocked.MockedGraspingRobotInterface diff --git a/perception/sandbox/polygrasp/conf/run_grasp.yaml b/perception/sandbox/polygrasp/conf/run_grasp.yaml new file mode 100644 index 0000000000..8e6e7ef51a --- /dev/null +++ b/perception/sandbox/polygrasp/conf/run_grasp.yaml @@ -0,0 +1,18 @@ +defaults: + - robot: robot_hw + - cam: cam_realsense + +masks_1: + - conf/masks/bin1/1.png + - conf/masks/bin1/2.png + - conf/masks/bin1/3.png +masks_2: + - conf/masks/bin2/1.png + - conf/masks/bin2/2.png + - conf/masks/bin2/3.png + +view_json_path: conf/o3d_view.json +min_mask_size: 2500 + +num_bin_shifts: 1 +num_grasps_per_bin_shift: 1 diff --git a/perception/sandbox/polygrasp/conf/run_ttp.yaml b/perception/sandbox/polygrasp/conf/run_ttp.yaml new file mode 100644 index 0000000000..50286a03e2 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/run_ttp.yaml @@ -0,0 +1,42 @@ +# @package _global_ +defaults: + - robot: robot_hw + - cam: cam_realsense + - trainer/padding_partial@padding: dual_model # input_pad_fn # pad_fn # prompt_pad_fn + # model + - trainer/submodule/category_encoder@category_encoder: mlp + - trainer/submodule/pose_encoder@pose_encoder: fourier_mlp + - trainer/submodule/temporal_encoder@temporal_encoder: embed + - trainer/submodule/reality_marker_encoder@reality_marker_encoder: embed + +checkpoint_path: '/home/yixinlin/temporal_task_planner/checkpoints/full_visible_two_pref.pt' +prompt_session_path: '/home/yixinlin/temporal_task_planner/artifacts/full_visible_env/pref_0123_456-top_first-True_back-center/2/sess_0.json' + +# model arch +category_embed_size: 64 +C_embed: 16 # category_embed_size // 4 +pose_embed_size: 128 +temporal_embed_size: 32 +marker_embed_size: 32 +d_model: 256 +d_hid: 512 +num_encoder_layers: 2 +num_decoder_layers: 2 +n_input_dim: 3 +num_slots: 50 +slot_iters: 3 + +masks_1: + - conf/masks/bin1/1.png + - conf/masks/bin1/2.png + - conf/masks/bin1/3.png +masks_2: + - conf/masks/bin2/1.png + - conf/masks/bin2/2.png + - conf/masks/bin2/3.png + +view_json_path: conf/o3d_view.json +min_mask_size: 2500 + +num_bin_shifts: 1 +num_grasps_per_bin_shift: 1 diff --git a/perception/sandbox/polygrasp/conf/trainer/GNN_model/Attention.yaml b/perception/sandbox/polygrasp/conf/trainer/GNN_model/Attention.yaml new file mode 100644 index 0000000000..91afef9b42 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/GNN_model/Attention.yaml @@ -0,0 +1,5 @@ +_target_: temporal_task_planner.trainer.GNN.GNN_policy.GATConvPolicySoftmax + +input_dim: ${input_dim} +hidden_dim: ${hidden_dim} +heads: 2 diff --git a/perception/sandbox/polygrasp/conf/trainer/GNN_model/GCN.yaml b/perception/sandbox/polygrasp/conf/trainer/GNN_model/GCN.yaml new file mode 100644 index 0000000000..9a6ce4169e --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/GNN_model/GCN.yaml @@ -0,0 +1,4 @@ +_target_: temporal_task_planner.trainer.GNN.GNN_policy.GCNPolicySoftmax + +input_dim: ${input_dim} +hidden_dim: ${hidden_dim} diff --git a/perception/sandbox/polygrasp/conf/trainer/criterion/dual_model.yaml b/perception/sandbox/polygrasp/conf/trainer/criterion/dual_model.yaml new file mode 100644 index 0000000000..b1f15641a1 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/criterion/dual_model.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.criterion.PickLoss +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/criterion/pick_loss.yaml b/perception/sandbox/polygrasp/conf/trainer/criterion/pick_loss.yaml new file mode 100644 index 0000000000..b1f15641a1 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/criterion/pick_loss.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.criterion.PickLoss +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/criterion/pick_place_loss.yaml b/perception/sandbox/polygrasp/conf/trainer/criterion/pick_place_loss.yaml new file mode 100644 index 0000000000..1d311951e2 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/criterion/pick_place_loss.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.criterion.PickPlaceLoss +device: ${device} +alpha: 0.5 \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/criterion/place_loss.yaml b/perception/sandbox/polygrasp/conf/trainer/criterion/place_loss.yaml new file mode 100644 index 0000000000..bf40246822 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/criterion/place_loss.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.criterion.PlaceLoss +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/criterion/preference_classifier.yaml b/perception/sandbox/polygrasp/conf/trainer/criterion/preference_classifier.yaml new file mode 100644 index 0000000000..c54555c744 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/criterion/preference_classifier.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.criterion.PreferenceClassificationLoss +device: ${device} diff --git a/perception/sandbox/polygrasp/conf/trainer/dataset_partial/dual_model.yaml b/perception/sandbox/polygrasp/conf/trainer/dataset_partial/dual_model.yaml new file mode 100644 index 0000000000..9c6cb7870b --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/dataset_partial/dual_model.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'PromptSituationPickPlace' +pick_only: ${pick_only} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/dataset_partial/preference_classifier.yaml b/perception/sandbox/polygrasp/conf/trainer/dataset_partial/preference_classifier.yaml new file mode 100644 index 0000000000..15233faed9 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/dataset_partial/preference_classifier.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'SessionPreferenceDataset' +pick_only: ${pick_only} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/dataset_partial/single_model.yaml b/perception/sandbox/polygrasp/conf/trainer/dataset_partial/single_model.yaml new file mode 100644 index 0000000000..5f3df4e550 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/dataset_partial/single_model.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'DishwasherArrangeDataset' +pick_only: ${pick_only} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/logger_partial/dual_model.yaml b/perception/sandbox/polygrasp/conf/trainer/logger_partial/dual_model.yaml new file mode 100644 index 0000000000..a9c785ac47 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/logger_partial/dual_model.yaml @@ -0,0 +1,9 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'DualModelLogger' +env: ${envs} +config: ${learner.config} +model: ${model} +criterion: ${criterion} +pick_only: ${pick_only} +max_evals: 10 +savefolder: ${rollout_savefolder} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/logger_partial/preference_classifier.yaml b/perception/sandbox/polygrasp/conf/trainer/logger_partial/preference_classifier.yaml new file mode 100644 index 0000000000..1273c947fc --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/logger_partial/preference_classifier.yaml @@ -0,0 +1,9 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'SessionPreferenceClassifierLogger' +env: ${envs} +config: ${learner.config} +model: ${model} +criterion: ${criterion} +pick_only: ${pick_only} +max_evals: 10 +savefolder: "rollouts" \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/logger_partial/single_model.yaml b/perception/sandbox/polygrasp/conf/trainer/logger_partial/single_model.yaml new file mode 100644 index 0000000000..8a1eea8ae9 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/logger_partial/single_model.yaml @@ -0,0 +1,8 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'SingleModelLogger' +config: ${learner.config} +model: ${model} +criterion: ${criterion} +pick_only: ${pick_only} +max_evals: ${max_evals} +savefolder: "rollouts" \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/optimizer_partial/adam.yaml b/perception/sandbox/polygrasp/conf/trainer/optimizer_partial/adam.yaml new file mode 100644 index 0000000000..f1d0add0d2 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/optimizer_partial/adam.yaml @@ -0,0 +1,8 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'Adam' +# params: model.parameters() +lr: ${lr} +betas: (0.9, 0.999) +# momentum: 0.9 +# weight_decay: 0.000001 +# dampening: 0.01 \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/optimizer_partial/sgd.yaml b/perception/sandbox/polygrasp/conf/trainer/optimizer_partial/sgd.yaml new file mode 100644 index 0000000000..33974fcc1e --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/optimizer_partial/sgd.yaml @@ -0,0 +1,7 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'SGD' +# params: model.parameters() +lr: ${lr} +momentum: 0.9 +# weight_decay: 0.0001 +# dampening: 0.1 diff --git a/perception/sandbox/polygrasp/conf/trainer/padding_partial/dual_model.yaml b/perception/sandbox/polygrasp/conf/trainer/padding_partial/dual_model.yaml new file mode 100644 index 0000000000..5e2073093f --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/padding_partial/dual_model.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'prompt_pad_fn' \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/padding_partial/input_pad_fn.yaml b/perception/sandbox/polygrasp/conf/trainer/padding_partial/input_pad_fn.yaml new file mode 100644 index 0000000000..5daad6b449 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/padding_partial/input_pad_fn.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: input_pad_fn \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/padding_partial/pad_fn.yaml b/perception/sandbox/polygrasp/conf/trainer/padding_partial/pad_fn.yaml new file mode 100644 index 0000000000..f8b86cd7e3 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/padding_partial/pad_fn.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: pad_fn \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/padding_partial/preference_classifier.yaml b/perception/sandbox/polygrasp/conf/trainer/padding_partial/preference_classifier.yaml new file mode 100644 index 0000000000..f12a13758a --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/padding_partial/preference_classifier.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: preference_classifier_pad_fn \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/padding_partial/prompt_pad_fn.yaml b/perception/sandbox/polygrasp/conf/trainer/padding_partial/prompt_pad_fn.yaml new file mode 100644 index 0000000000..5e2073093f --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/padding_partial/prompt_pad_fn.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'prompt_pad_fn' \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/scheduler_partial/exponential_lr.yaml b/perception/sandbox/polygrasp/conf/trainer/scheduler_partial/exponential_lr.yaml new file mode 100644 index 0000000000..8c7838beb8 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/scheduler_partial/exponential_lr.yaml @@ -0,0 +1,5 @@ +_target_: temporal_task_planner.trainer.module_wrapper.ModuleWrapper +method_name: 'ExponentialLR' +# optimizer: ${optimizer} +gamma: 0.995 +last_epoch: -1 diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/dummy.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/dummy.yaml new file mode 100644 index 0000000000..86875dfaa0 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/dummy.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.dummy_input.DummyInput +d_model: ${category_embed_size} +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/fourier.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/fourier.yaml new file mode 100644 index 0000000000..93e8abf821 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/fourier.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.category_encoder.CategoryEncoderFourier +h_dim: ${category_embed_size} +C_embed: ${C_embed} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/fourier_mlp.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/fourier_mlp.yaml new file mode 100644 index 0000000000..fcb249ef61 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/fourier_mlp.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.category_encoder.CategoryEncoderFourierMLP +h_dim: ${category_embed_size} +C_embed: ${C_embed} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/mlp.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/mlp.yaml new file mode 100644 index 0000000000..b7e3eb0c47 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/category_encoder/mlp.yaml @@ -0,0 +1,2 @@ +_target_: temporal_task_planner.trainer.submodule.category_encoder.CategoryEncoderMLP +h_dim: ${category_embed_size} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/dummy.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/dummy.yaml new file mode 100644 index 0000000000..81e9b012ac --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/dummy.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.dummy_input.DummyInput +d_model: ${pose_embed_size} +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/fourier_mlp.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/fourier_mlp.yaml new file mode 100644 index 0000000000..28181a4071 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/fourier_mlp.yaml @@ -0,0 +1,4 @@ +_target_: temporal_task_planner.trainer.submodule.pose_encoder.PoseEncoderFourierMLP +P_embed: 3 +d_model: ${pose_embed_size} +n_input_dim: ${n_input_dim} diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/mlp.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/mlp.yaml new file mode 100644 index 0000000000..6d586cce95 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/pose_encoder/mlp.yaml @@ -0,0 +1,4 @@ +_target_: temporal_task_planner.trainer.submodule.pose_encoder.PoseEncoderMLP +P_embed: 3 +d_model: ${pose_embed_size} +n_input_dim: ${n_input_dim} diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/reality_marker_encoder/dummy.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/reality_marker_encoder/dummy.yaml new file mode 100644 index 0000000000..8a2d68369b --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/reality_marker_encoder/dummy.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.dummy_input.DummyInput +d_model: ${marker_embed_size} +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/reality_marker_encoder/embed.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/reality_marker_encoder/embed.yaml new file mode 100644 index 0000000000..e663ebcd50 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/reality_marker_encoder/embed.yaml @@ -0,0 +1,3 @@ +_target_: torch.nn.Embedding +num_embeddings: 2 +embedding_dim: ${marker_embed_size} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/dummy.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/dummy.yaml new file mode 100644 index 0000000000..3d34016542 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/dummy.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.dummy_input.DummyInput +d_model: ${temporal_embed_size} +device: ${device} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/embed.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/embed.yaml new file mode 100644 index 0000000000..d9e75d36bd --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/embed.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.temporal_encoder.TemporalEncoderEmbedding +T_embed: 50 +d_model: ${temporal_embed_size} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/fourier.yaml b/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/fourier.yaml new file mode 100644 index 0000000000..aee0620116 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/submodule/temporal_encoder/fourier.yaml @@ -0,0 +1,3 @@ +_target_: temporal_task_planner.trainer.submodule.temporal_encoder.TemporalEncoderFourierMLP +T_embed: 5 +d_model: ${temporal_embed_size} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/transformer_model/config.yaml b/perception/sandbox/polygrasp/conf/trainer/transformer_model/config.yaml new file mode 100644 index 0000000000..faf0c56cc0 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/transformer_model/config.yaml @@ -0,0 +1,15 @@ +_target_: temporal_task_planner.trainer.transformer.configs.TransformerTaskPlannerConfig +num_instances: 60 +d_model: ${d_model} +nhead: 2 +d_hid: ${d_hid} +num_slots: ${num_slots} +slot_iters: ${slot_iters} +num_encoder_layers: ${num_encoder_layers} +num_decoder_layers: ${num_decoder_layers} +dropout: 0. +batch_first: True +category_embed_size: ${category_embed_size} +pose_embed_size: ${pose_embed_size} +temporal_embed_size: ${temporal_embed_size} +marker_embed_size: ${marker_embed_size} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/transformer_model/dual_model.yaml b/perception/sandbox/polygrasp/conf/trainer/transformer_model/dual_model.yaml new file mode 100644 index 0000000000..dc5e462a26 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/transformer_model/dual_model.yaml @@ -0,0 +1,24 @@ +# @package _global_ +_target_: temporal_task_planner.trainer.transformer.dual_model.TransformerTaskPlannerDualModel + +config: + _target_: temporal_task_planner.trainer.transformer.configs.TransformerTaskPlannerConfig + num_instances: 60 + d_model: ${d_model} + nhead: 2 + d_hid: ${d_hid} + num_slots: ${num_slots} + slot_iters: ${slot_iters} + num_encoder_layers: ${num_encoder_layers} + num_decoder_layers: ${num_decoder_layers} + dropout: 0. + batch_first: True + category_embed_size: ${category_embed_size} + pose_embed_size: ${pose_embed_size} + temporal_embed_size: ${temporal_embed_size} + marker_embed_size: ${marker_embed_size} + +category_encoder: ${category_encoder} +pose_encoder: ${pose_encoder} +temporal_encoder: ${temporal_encoder} +reality_marker_encoder: ${reality_marker_encoder} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/transformer_model/preference_classifier.yaml b/perception/sandbox/polygrasp/conf/trainer/transformer_model/preference_classifier.yaml new file mode 100644 index 0000000000..7fc3302432 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/transformer_model/preference_classifier.yaml @@ -0,0 +1,23 @@ +_target_: temporal_task_planner.trainer.transformer.single_model.PreferenceClassifier + +config: + _target_: temporal_task_planner.trainer.transformer.configs.TransformerTaskPlannerConfig + num_instances: 60 + d_model: ${d_model} + nhead: 2 + d_hid: ${d_hid} + num_encoder_layers: ${num_encoder_layers} + dropout: 0. + batch_first: True + num_pref: ${num_pref} + num_slots: ${num_slots} + slot_iters: ${slot_iters} + category_embed_size: ${category_embed_size} + pose_embed_size: ${pose_embed_size} + temporal_embed_size: ${temporal_embed_size} + marker_embed_size: ${marker_embed_size} + +category_encoder: ${category_encoder} +pose_encoder: ${pose_encoder} +temporal_encoder: ${temporal_encoder} +reality_marker_encoder: ${reality_marker_encoder} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/conf/trainer/transformer_model/single_model.yaml b/perception/sandbox/polygrasp/conf/trainer/transformer_model/single_model.yaml new file mode 100644 index 0000000000..f750dbcfb6 --- /dev/null +++ b/perception/sandbox/polygrasp/conf/trainer/transformer_model/single_model.yaml @@ -0,0 +1,22 @@ +_target_: temporal_task_planner.trainer.transformer.single_model.TransformerTaskPlannerSingleModel + +config: + _target_: temporal_task_planner.trainer.transformer.configs.TransformerTaskPlannerConfig + num_instances: 60 + d_model: ${d_model} + nhead: 2 + d_hid: ${d_hid} + num_encoder_layers: ${num_encoder_layers} + dropout: 0. + batch_first: True + num_pref: ${num_pref} + num_slots: ${num_slots} + slot_iters: ${slot_iters} + category_embed_size: ${category_embed_size} + pose_embed_size: ${pose_embed_size} + temporal_embed_size: ${temporal_embed_size} + marker_embed_size: ${marker_embed_size} +category_encoder: ${category_encoder} +pose_encoder: ${pose_encoder} +temporal_encoder: ${temporal_encoder} +reality_marker_encoder: ${reality_marker_encoder} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/all_closed.json b/perception/sandbox/polygrasp/data/all_closed.json new file mode 100644 index 0000000000..3c441aa3bd --- /dev/null +++ b/perception/sandbox/polygrasp/data/all_closed.json @@ -0,0 +1,67 @@ +{ + "all_closed": [ + { + "id": 27, + "corner": [ + [ + 103, + 416 + ], + [ + 103, + 380 + ], + [ + 139, + 378 + ], + [ + 138, + 412 + ] + ] + }, + { + "id": 29, + "corner": [ + [ + 239, + 376 + ], + [ + 270, + 376 + ], + [ + 269, + 407 + ], + [ + 238, + 407 + ] + ] + }, + { + "id": 23, + "corner": [ + [ + 200, + 128 + ], + [ + 200, + 156 + ], + [ + 179, + 160 + ], + [ + 182, + 132 + ] + ] + } + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/all_open.json b/perception/sandbox/polygrasp/data/all_open.json new file mode 100644 index 0000000000..b7dca2cf3b --- /dev/null +++ b/perception/sandbox/polygrasp/data/all_open.json @@ -0,0 +1,25 @@ +{ + "all_open": [ + { + "id": 29, + "corner": [ + [ + 401, + 415 + ], + [ + 438, + 415 + ], + [ + 438, + 455 + ], + [ + 401, + 455 + ] + ] + } + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/category_to_rgb_map.json b/perception/sandbox/polygrasp/data/category_to_rgb_map.json new file mode 100644 index 0000000000..af82a2167c --- /dev/null +++ b/perception/sandbox/polygrasp/data/category_to_rgb_map.json @@ -0,0 +1,91 @@ +{ + "yellow_cup": [ + [ + 0.6431372549019608, + 0.4980392156862745, + 0.0 + ] + ], + "dark_blue_plate": [ + [ + 0.06666666666666667, + 0.3176470588235294, + 0.42745098039215684 + ] + ], + "light_blue_bowl": [ + [ + 0.23921568627450981, + 0.6823529411764706, + 0.792156862745098 + ] + ], + "pink_small_bowl": [ + [ + 0.6972549019607844, + 0.26666678431372549019, + 0.25882353615686274509804 + ] + ], + "peach_big_bowl": [ + [ + 0.7372549019607844, + 0.6078431372549019, + 0.615686274509804 + ] + ], + "pink_plate": [ + 0.7372549 , 0.34117647, 0.58039216 + ], + "purple_plate": [ + 0.30980392, 0.15294118, 0.4627451 + ], + "aqua_plate": [ + 0.42352941, 0.70588235, 0.71372549 + ], + "neon_yellow_plate": [ + 0.6627451 , 0.73333333, 0.39215686 + ], + "ignore_shadow_0": [ + [ + 0.5333333333333333, + 0.5411764705882353, + 0.5294117647058824 + ] + ], + "red_bowl": [ + [ + 0.5450980392156862, + 0.0392156862745098, + 0.03529411764705882 + ] + ], + "ignore_shadow_1": [ + [ + 0.16470588235294117, + 0.1607843137254902, + 0.050980392156862744 + ] + ], + "ignore_camera_stand": [ + [ + 0.0, + 0.050980392156862744, + 0.00784313725490196 + ] + ], + "ignore_shadow_3": [ + [ + 0.6078431372549019, + 0.611764705882353, + 0.6862745098039216 + ] + ], + "ignore_sink": [ + [ + 0.28627451, + 0.55294118, + 0.10196078 + ] + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/category_to_rgb_map_2.json b/perception/sandbox/polygrasp/data/category_to_rgb_map_2.json new file mode 100644 index 0000000000..96fa0f4f3b --- /dev/null +++ b/perception/sandbox/polygrasp/data/category_to_rgb_map_2.json @@ -0,0 +1,4 @@ +{"red_bowl": [[0.5803921568627451, 0.00784313725490196, 0.0]], + "yellow_cup": [[0.7019607843137254, 0.5647058823529412, 0.0]], "dark_blue_plate": [[0.054901960784313725, 0.32941176470588235, 0.4117647058823529]], + "pink_bowl": [[0.7372549019607844, 0.6078431372549019, 0.615686274509804]], + "light_blue_bowl": [[0.23529411764705882, 0.6392156862745098, 0.7215686274509804]], "green_cup": [[0.3058823529411765, 0.5058823529411764, 0.20784313725490197]], "ignore": [[0.0, 0.6274509803921569, 0.7058823529411765]], "ignore": [[0.5333333333333333, 0.5529411764705883, 0.5568627450980392]], "ignore": [[0.13333333333333333, 0.16470588235294117, 0.050980392156862744]], "ignore": [[0.5647058823529412, 0.5647058823529412, 0.4470588235294118]]} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/drawer_status_ar_tag.json b/perception/sandbox/polygrasp/data/drawer_status_ar_tag.json new file mode 100644 index 0000000000..01b6b79bf9 --- /dev/null +++ b/perception/sandbox/polygrasp/data/drawer_status_ar_tag.json @@ -0,0 +1,125 @@ +{ + "top-open": { + "id": 26, + "corner": [ + [278, 376], + [277, 343], + [302, 335], + [305, 368] + ] + }, + "bottom-open": [ + { + "id": 23, + "corner": [ + [ + 212, + 344 + ], + [ + 237, + 336 + ], + [ + 241, + 367 + ], + [ + 215, + 375 + ] + ] + }, + { + "id": 27, + "corner": [ + [ + 232, + 66 + ], + [ + 237, + 106 + ], + [ + 211, + 110 + ], + [ + 204, + 72 + ] + ] + } + + ], + "top-closed_bottom-closed": [ + { + "id": 23, + "corner": [ + [ + 360, + 303 + ], + [ + 376, + 299 + ], + [ + 379, + 323 + ], + [ + 362, + 329 + ] + ] + }, + { + "id": 29, + "corner": [ + [ + 301, + 121 + ], + [ + 283, + 124 + ], + [ + 282, + 96 + ], + [ + 299, + 92 + ] + ] + }, + { + "id": 27, + "corner": [ + [ + 433, + 80 + ], + [ + 435, + 110 + ], + [ + 410, + 113 + ], + [ + 407, + 85 + ] + ] + } + ] + + + + +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/hw_points_1 copy.json b/perception/sandbox/polygrasp/data/hw_points_1 copy.json new file mode 100644 index 0000000000..800bbcacae --- /dev/null +++ b/perception/sandbox/polygrasp/data/hw_points_1 copy.json @@ -0,0 +1,22 @@ +{ + "red_bowl": [ + 0.4649015880465838, + -0.2502910799518351, + 0.03723051417968672 + ], + "dark_blue_plate": [ + 0.36121687347236575, + -0.4303462147053958, + 0.06668189721727034 + ], + "light_blue_bowl": [ + 0.6790145716044703, + -0.2882750025793654, + 0.03315757327523252 + ], + "yellow_cup": [ + 0.5567961834840846, + -0.45522138863053024, + 0.0951139496498279 + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/hw_points_1.json b/perception/sandbox/polygrasp/data/hw_points_1.json new file mode 100644 index 0000000000..daa8ded931 --- /dev/null +++ b/perception/sandbox/polygrasp/data/hw_points_1.json @@ -0,0 +1,22 @@ +{ + "light_blue_bowl": [ + 0.6790771757598185, + -0.2881603135454016, + 0.03297779390255586 + ], + "yellow_cup": [ + 0.5570186661051413, + -0.45124384835595605, + 0.09196449605933966 + ], + "dark_blue_plate": [ + 0.36146080922148605, + -0.4290290268349362, + 0.06672253306931498 + ], + "red_bowl": [ + 0.46471973929810845, + -0.25421831047504784, + 0.035634863321517385 + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/hw_points_2.json b/perception/sandbox/polygrasp/data/hw_points_2.json new file mode 100644 index 0000000000..a370bf6e3a --- /dev/null +++ b/perception/sandbox/polygrasp/data/hw_points_2.json @@ -0,0 +1,22 @@ +{ + "yellow_cup": [ + 0.3203656951778264, + -0.28067178238129914, + 0.09580121589281672 + ], + "light_blue_bowl": [ + 0.5658876402116196, + -0.23404818642208414, + 0.03726189678094269 + ], + "red_bowl": [ + 0.6536752158871245, + -0.4128134796902002, + 0.0277001752288103 + ], + "dark_blue_plate": [ + 0.43184164995144886, + -0.4211320980530253, + 0.06651915508958957 + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/hw_points_3.json b/perception/sandbox/polygrasp/data/hw_points_3.json new file mode 100644 index 0000000000..7dead866da --- /dev/null +++ b/perception/sandbox/polygrasp/data/hw_points_3.json @@ -0,0 +1,22 @@ +{ + "dark_blue_plate": [ + 0.6624394397302245, + -0.4375997077783539, + 0.06466443636180626 + ], + "light_blue_bowl": [ + 0.36990778881462444, + -0.3343705224296166, + 0.036031637078604996 + ], + "red_bowl": [ + 0.5626016827532018, + -0.24009111889017482, + 0.03548705277004566 + ], + "yellow_cup": [ + 0.3424850804914824, + -0.508392704708471, + 0.08568477520160946 + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/hw_points_top_drawer.json b/perception/sandbox/polygrasp/data/hw_points_top_drawer.json new file mode 100644 index 0000000000..05ac7433b3 --- /dev/null +++ b/perception/sandbox/polygrasp/data/hw_points_top_drawer.json @@ -0,0 +1,12 @@ +{ + "red_bowl": [ + 0.5630083928294418, + -0.23860095201346504, + 0.033132792459696886 + ], + "light_blue_bowl": [ + 0.3678457371258019, + -0.33672955797480264, + 0.03607353081096594 + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/open_bottom_drawer.json b/perception/sandbox/polygrasp/data/open_bottom_drawer.json new file mode 100644 index 0000000000..f9e37705dd --- /dev/null +++ b/perception/sandbox/polygrasp/data/open_bottom_drawer.json @@ -0,0 +1,67 @@ +{ + "open_bottom_drawer": [ + { + "id": 29, + "corner": [ + [ + 401, + 415 + ], + [ + 438, + 416 + ], + [ + 438, + 455 + ], + [ + 401, + 455 + ] + ] + }, + { + "id": 27, + "corner": [ + [ + 77, + 431 + ], + [ + 78, + 398 + ], + [ + 113, + 397 + ], + [ + 111, + 430 + ] + ] + }, + { + "id": 23, + "corner": [ + [ + 341, + 113 + ], + [ + 342, + 147 + ], + [ + 313, + 152 + ], + [ + 315, + 119 + ] + ] + } + ] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/open_top_drawer.json b/perception/sandbox/polygrasp/data/open_top_drawer.json new file mode 100644 index 0000000000..bab8501306 --- /dev/null +++ b/perception/sandbox/polygrasp/data/open_top_drawer.json @@ -0,0 +1,3 @@ +{ + "open_top_drawer": [] +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/data/pcd_outlier_detect.py b/perception/sandbox/polygrasp/data/pcd_outlier_detect.py new file mode 100644 index 0000000000..2256492b30 --- /dev/null +++ b/perception/sandbox/polygrasp/data/pcd_outlier_detect.py @@ -0,0 +1,7 @@ +{ + 'light_blue_bowl': [0.17027859, 0.16715852, 0.06790836], + 'yellow_cup': [0.14412579, 0.11836442, 0.0826261 ], + array([0.08826146, 0.08102679, 0.12599949]) + + +} diff --git a/perception/sandbox/polygrasp/data/rgbd.npy b/perception/sandbox/polygrasp/data/rgbd.npy new file mode 100644 index 0000000000..ecb3f379fc Binary files /dev/null and b/perception/sandbox/polygrasp/data/rgbd.npy differ diff --git a/perception/sandbox/polygrasp/env_polygrasp.yml b/perception/sandbox/polygrasp/env_polygrasp.yml new file mode 100644 index 0000000000..077e98af28 --- /dev/null +++ b/perception/sandbox/polygrasp/env_polygrasp.yml @@ -0,0 +1,380 @@ +name: polygrasp +channels: + - fair-robotics + - aihabitat + - pytorch + - conda-forge + - defaults +dependencies: + - _libgcc_mutex=0.1=conda_forge + - _openmp_mutex=4.5=2_gnu + - abseil-cpp=20210324.2=h9c3ff4c_0 + - aiohttp=3.8.1=py38h0a891b7_1 + - aiosignal=1.2.0=pyhd8ed1ab_0 + - alabaster=0.7.12=py_0 + - alsa-lib=1.2.3.2=h166bdaf_0 + - antlr-python-runtime=4.8=py38h32f6830_2 + - assimp=5.0.1=h286c4ac_5 + - asv=0.5.1=py38h709712a_0 + - async-timeout=4.0.2=pyhd8ed1ab_0 + - attrs=21.4.0=pyhd8ed1ab_0 + - babel=2.10.1=pyhd8ed1ab_0 + - backports=1.0=py_2 + - backports.functools_lru_cache=1.6.4=pyhd8ed1ab_0 + - beautifulsoup4=4.11.1=pyha770c72_0 + - black=22.3.0=pyhd8ed1ab_0 + - blas=1.0=mkl + - boost=1.72.0=py38h1e42940_1 + - boost-cpp=1.72.0=h312852a_5 + - breathe=4.33.0=pyhd8ed1ab_0 + - brotli=1.0.9=h166bdaf_7 + - brotli-bin=1.0.9=h166bdaf_7 + - brotli-python=1.0.9=py38hfa26641_7 + - brotlipy=0.7.0=py38h0a891b7_1004 + - bullet-cpp=3.17=h43a58ef_1 + - bzip2=1.0.8=h7f98852_4 + - c-ares=1.18.1=h7f98852_0 + - ca-certificates=2022.5.18.1=ha878542_0 + - certifi=2022.5.18.1=py38h578d9bd_0 + - charset-normalizer=2.0.12=pyhd8ed1ab_0 + - click=8.1.3=py38h578d9bd_0 + - cmake=3.22.1=h1fce559_0 + - colorama=0.4.4=pyh9f0ad1d_0 + - console_bridge=1.0.2=h924138e_1 + - cryptography=36.0.2=py38h2b5fc30_1 + - cudatoolkit=11.3.1=ha36c431_10 + - cycler=0.11.0=pyhd8ed1ab_0 + - dash=2.4.1=pyhd8ed1ab_0 + - dataclasses=0.8=pyhc8e2a94_3 + - dbus=1.13.6=h48d8840_2 + - docutils=0.17.1=py38h578d9bd_2 + - doxygen=1.9.2=hb166930_0 + - eigen=3.4.0=h4bd325d_0 + - eigenpy=2.6.0=py38h43b7125_0 + - expat=2.4.8=h27087fc_0 + - ffmpeg=4.3=hf484d3e_0 + - flask=2.1.2=pyhd8ed1ab_1 + - flask-compress=1.12=pyhd8ed1ab_0 + - fontconfig=2.14.0=h8e229c2_0 + - fonttools=4.33.3=py38h0a891b7_0 + - freetype=2.10.4=h0708190_1 + - frozenlist=1.3.0=py38h0a891b7_1 + - gettext=0.19.8.1=h0b5b191_1005 + - giflib=5.2.1=h36c2ea0_2 + - gitdb=4.0.9=pyhd8ed1ab_0 + - gitpython=3.1.27=pyhd8ed1ab_0 + - glib=2.68.4=h9c3ff4c_0 + - glib-tools=2.68.4=h9c3ff4c_0 + - gmp=6.2.1=h58526e2_0 + - gnutls=3.6.13=h85f3911_1 + - grpc-cpp=1.41.1=h75e9d12_2 + - grpcio=1.46.1=py38ha0cdfde_0 + - gst-plugins-base=1.18.5=hf529b03_0 + - gstreamer=1.18.5=h76c114f_0 + - habitat-sim-mutex=1.0=display_bullet + - hpp-fcl=1.6.0=py38h480e1e4_3 + - hydra-core=1.0.6=pyhd8ed1ab_1 + - icu=68.2=h9c3ff4c_0 + - idna=3.3=pyhd8ed1ab_0 + - imageio-ffmpeg=0.4.7=pyhd8ed1ab_0 + - imagesize=1.3.0=pyhd8ed1ab_0 + - importlib-metadata=4.11.3=py38h578d9bd_1 + - importlib_resources=5.7.1=pyhd8ed1ab_1 + - iniconfig=1.1.1=pyh9f0ad1d_0 + - intel-openmp=2022.0.1=h06a4308_3633 + - itsdangerous=2.1.2=pyhd8ed1ab_0 + - jbig=2.1=h7f98852_2003 + - jinja2=3.1.2=pyhd8ed1ab_0 + - jpeg=9e=h166bdaf_1 + - keyutils=1.6.1=h166bdaf_0 + - kiwisolver=1.4.2=py38h43d8883_1 + - krb5=1.19.3=h3790be6_0 + - lame=3.100=h7f98852_1001 + - lcms2=2.12=hddcbb42_0 + - ld_impl_linux-64=2.38=h1181459_0 + - lerc=3.0=h9c3ff4c_0 + - libblas=3.9.0=14_linux64_mkl + - libbrotlicommon=1.0.9=h166bdaf_7 + - libbrotlidec=1.0.9=h166bdaf_7 + - libbrotlienc=1.0.9=h166bdaf_7 + - libcblas=3.9.0=14_linux64_mkl + - libclang=11.1.0=default_ha53f305_1 + - libcurl=7.82.0=h0b77cf5_0 + - libdeflate=1.10=h7f98852_0 + - libedit=3.1.20191231=he28a2e2_2 + - libev=4.33=h7f8727e_1 + - libevent=2.1.10=h9b69904_4 + - libffi=3.3=he6710b0_2 + - libgcc=7.2.0=h69d50b8_2 + - libgcc-ng=12.1.0=h8d9b700_16 + - libgfortran-ng=12.1.0=h69a702a_16 + - libgfortran5=12.1.0=hdcd56e2_16 + - libglib=2.68.4=h3e27bee_0 + - libgomp=12.1.0=h8d9b700_16 + - libiconv=1.16=h516909a_0 + - liblapack=3.9.0=14_linux64_mkl + - libllvm11=11.1.0=hf817b99_3 + - libnghttp2=1.46.0=hce63b2e_0 + - libogg=1.3.4=h7f98852_1 + - libopus=1.3.1=h7f98852_1 + - libpng=1.6.37=h21135ba_2 + - libpq=13.5=hd57d9b9_1 + - libprotobuf=3.18.1=h780b84a_0 + - librealsense=2.50.0=py3.8 + - libssh2=1.10.0=h8f2d780_0 + - libstdcxx-ng=12.1.0=ha89aaad_16 + - libtiff=4.3.0=h542a066_3 + - libudev1=249=h166bdaf_2 + - libusb=1.0.26=h0b4f0b6_100 + - libuuid=2.32.1=h7f98852_1000 + - libuv=1.43.0=h7f98852_0 + - libvorbis=1.3.7=h9c3ff4c_0 + - libwebp=1.2.2=h3452ae3_0 + - libwebp-base=1.2.2=h7f98852_1 + - libxcb=1.13=h7f98852_1004 + - libxkbcommon=1.0.3=he3ba5ed_0 + - libxml2=2.9.12=h72842e0_0 + - libzlib=1.2.11=h166bdaf_1014 + - llvmlite=0.38.0=py38h38d86a4_1 + - lz4-c=1.9.3=h9c3ff4c_1 + - markdown-it-py=2.1.0=pyhd8ed1ab_0 + - markupsafe=2.1.1=py38h0a891b7_1 + - matplotlib=3.5.2=py38h578d9bd_0 + - matplotlib-base=3.5.2=py38h826bfd8_0 + - mdit-py-plugins=0.3.0=pyhd8ed1ab_0 + - mdurl=0.1.0=pyhd8ed1ab_0 + - mkl=2022.0.1=h06a4308_117 + - multidict=6.0.2=py38h0a891b7_1 + - munkres=1.1.4=pyh9f0ad1d_0 + - mypy_extensions=0.4.3=py38h578d9bd_5 + - mysql-common=8.0.29=haf5c9bc_0 + - mysql-libs=8.0.29=h28c427c_0 + - myst-parser=0.17.2=pyhd8ed1ab_0 + - ncurses=6.3=h7f8727e_2 + - nettle=3.6=he412f7d_0 + - nspr=4.32=h9c3ff4c_1 + - nss=3.77=h2350873_0 + - numba=0.55.1=py38hdc3674a_1 + - octomap=1.9.7=h4bd325d_0 + - omegaconf=2.0.6=py38h578d9bd_0 + - openh264=2.1.1=h780b84a_0 + - openjpeg=2.4.0=hb52868f_1 + - openmpi=2.1.2=0 + - openssl=1.1.1o=h166bdaf_0 + - packaging=21.3=pyhd8ed1ab_0 + - pathspec=0.9.0=pyhd8ed1ab_0 + - pcre=8.45=h9c3ff4c_0 + - pillow=9.1.0=py38h0ee0e06_2 + - pinocchio=2.5.2=py38ha65b770_0 + - pip=21.2.4=py38h06a4308_0 + - platformdirs=2.5.1=pyhd8ed1ab_0 + - plotly=5.8.0=pyhd8ed1ab_0 + - pluggy=1.0.0=py38h578d9bd_3 + - poco=1.9.0=h876a3cc_2003 + - prompt-toolkit=3.0.29=pyha770c72_0 + - prompt_toolkit=3.0.29=hd8ed1ab_0 + - protobuf=3.18.1=py38h709712a_0 + - pthread-stubs=0.4=h36c2ea0_1001 + - py=1.11.0=pyh6c4a22f_0 + - py-cpuinfo=8.0.0=pyhd8ed1ab_0 + - pybind11=2.9.2=py38h43d8883_1 + - pybind11-global=2.9.2=py38h43d8883_1 + - pybullet=3.17=py38h43a58ef_1 + - pycparser=2.21=pyhd8ed1ab_0 + - pydata-sphinx-theme=0.8.1=pyhd8ed1ab_0 + - pygments=2.12.0=pyhd8ed1ab_0 + - pymodbus=2.5.3=pyhd8ed1ab_0 + - pyopenssl=22.0.0=pyhd8ed1ab_0 + - pyparsing=3.0.9=pyhd8ed1ab_0 + - pyqt=5.12.3=py38h578d9bd_8 + - pyqt-impl=5.12.3=py38h0ffb2e6_8 + - pyqt5-sip=4.19.18=py38h709712a_8 + - pyqtchart=5.12=py38h7400c14_8 + - pyqtwebengine=5.12.1=py38h7400c14_8 + - pyserial=3.5=pyhd8ed1ab_0 + - pyserial-asyncio=0.6=pyhd8ed1ab_0 + - pysocks=1.7.1=py38h578d9bd_5 + - pytest=7.1.2=py38h578d9bd_0 + - pytest-benchmark=3.4.1=pyhd8ed1ab_0 + - python=3.8.13=h12debd9_0 + - python-dateutil=2.8.2=pyhd8ed1ab_0 + - python_abi=3.8=2_cp38 + - pytorch=1.10.0=py3.8_cuda11.3_cudnn8.2.0_0 + - pytorch-mutex=1.0=cuda + - pytz=2022.1=pyhd8ed1ab_0 + - pyyaml=6.0=py38h0a891b7_4 + - qt=5.12.9=hda022c4_4 + - quaternion=2022.4.2=py38h71d37f0_0 + - re2=2021.11.01=h9c3ff4c_0 + - readline=8.1.2=h7f8727e_1 + - requests=2.27.1=pyhd8ed1ab_0 + - rhash=1.4.1=h3c74f83_1 + - scipy=1.8.0=py38h56a6a73_1 + - setuptools=61.2.0=py38h06a4308_0 + - six=1.16.0=pyh6c4a22f_0 + - smmap=3.0.5=pyh44b312d_0 + - snowballstemmer=2.2.0=pyhd8ed1ab_0 + - spdlog=1.10.0=h924138e_0 + - sphinx=4.5.0=pyh6c4a22f_0 + - sphinx-book-theme=0.3.2=pyhd8ed1ab_0 + - sphinxcontrib-applehelp=1.0.2=py_0 + - sphinxcontrib-devhelp=1.0.2=py_0 + - sphinxcontrib-htmlhelp=2.0.0=pyhd8ed1ab_0 + - sphinxcontrib-jsmath=1.0.1=py_0 + - sphinxcontrib-qthelp=1.0.3=py_0 + - sphinxcontrib-serializinghtml=1.1.5=pyhd8ed1ab_2 + - sqlite=3.38.5=h4ff8645_0 + - tenacity=8.0.1=pyhd8ed1ab_0 + - tinyxml=2.6.2=h4bd325d_2 + - tk=8.6.12=h27826a3_0 + - tomli=2.0.1=pyhd8ed1ab_0 + - tornado=6.1=py38h0a891b7_3 + - tqdm=4.64.0=pyhd8ed1ab_0 + - typed-ast=1.5.3=py38h0a891b7_0 + - typing-extensions=4.2.0=hd8ed1ab_1 + - typing_extensions=4.2.0=pyha770c72_1 + - unicodedata2=14.0.0=py38h0a891b7_1 + - unixodbc=2.3.10=h583eb01_0 + - urdfdom=2.3.5=h4bd325d_0 + - urdfdom_headers=1.0.6=h924138e_2 + - urllib3=1.26.9=pyhd8ed1ab_0 + - wcwidth=0.2.5=pyh9f0ad1d_2 + - werkzeug=2.1.2=pyhd8ed1ab_1 + - wheel=0.37.1=pyhd3eb1b0_0 + - withbullet=2.0=0 + - xorg-fixesproto=5.0=h7f98852_1002 + - xorg-inputproto=2.3.2=h7f98852_1002 + - xorg-kbproto=1.0.7=h7f98852_1002 + - xorg-libx11=1.7.2=h7f98852_0 + - xorg-libxau=1.0.9=h7f98852_0 + - xorg-libxcursor=1.2.0=h7f98852_0 + - xorg-libxdmcp=1.1.3=h7f98852_0 + - xorg-libxext=1.3.4=h7f98852_1 + - xorg-libxfixes=5.0.3=h7f98852_1004 + - xorg-libxi=1.7.10=h7f98852_0 + - xorg-libxinerama=1.1.4=h9c3ff4c_1001 + - xorg-libxrandr=1.5.2=h7f98852_1 + - xorg-libxrender=0.9.10=h7f98852_1003 + - xorg-randrproto=1.5.0=h7f98852_1001 + - xorg-renderproto=0.11.1=h7f98852_1002 + - xorg-xextproto=7.3.0=h7f98852_1002 + - xorg-xproto=7.0.31=h7f98852_1007 + - xz=5.2.5=h7f8727e_1 + - yaml=0.2.5=h7f98852_2 + - yaml-cpp=0.6.3=he1b5a44_4 + - yarl=1.7.2=py38h0a891b7_2 + - zipp=3.8.0=pyhd8ed1ab_0 + - zlib=1.2.11=h166bdaf_1014 + - zstd=1.5.2=ha95c52a_0 + - pip: + - addict==2.4.0 + - aiodocker==0.21.0 + - alephzero==0.3.16 + - anyio==3.6.1 + - argon2-cffi==21.3.0 + - argon2-cffi-bindings==21.2.0 + - asttokens==2.0.5 + - autolab-core==1.1.1 + - autolab-perception==1.0.0 + - backcall==0.2.0 + - bleach==5.0.0 + - cffi==1.15.0 + - colorlog==6.6.0 + - cvxopt==1.3.0 + - cython==0.29.29 + - debugpy==1.6.0 + - decorator==5.1.1 + - defusedxml==0.7.1 + - deprecation==2.1.0 + - dill==0.3.4 + - docker==5.0.3 + - entrypoints==0.4 + - executing==0.8.3 + - fastjsonschema==2.15.3 + - ffmpeg-python==0.2.0 + - future==0.18.2 + - grasp-nms==1.0.2 + - graspnetapi==1.2.10 + - gtsam==4.1.1 + - h5py==3.6.0 + - habitat-sim==0.2.1 + - ikpy==3.3.3 + - imageio==2.19.2 + - ipdb==0.13.9 + - ipykernel==6.13.0 + - ipython==8.3.0 + - ipython-genutils==0.2.0 + - ipywidgets==7.7.0 + - jedi==0.18.1 + - joblib==1.1.0 + - json5==0.9.8 + - jsonpointer==2.3 + - jsonschema==4.5.1 + - jupyter-client==7.3.1 + - jupyter-core==4.10.0 + - jupyter-packaging==0.12.0 + - jupyter-server==1.17.0 + - jupyterlab==3.4.2 + - jupyterlab-pygments==0.2.2 + - jupyterlab-server==2.13.0 + - jupyterlab-widgets==1.1.0 + - klampt==0.9.0 + - matplotlib-inline==0.1.3 + - mistune==0.8.4 + - mpmath==1.2.1 + - mrp==0.1.9 + - multiprocess==0.70.12.2 + - nbclassic==0.3.7 + - nbclient==0.6.3 + - nbconvert==6.5.0 + - nbformat==5.4.0 + - nest-asyncio==1.5.5 + - networkx==2.8 + - notebook==6.4.11 + - notebook-shim==0.1.0 + - numpy==1.22.3 + - open3d==0.15.2 + - opencv-contrib-python==4.5.5.64 + - opencv-python==4.5.5.64 + - pandas==1.4.2 + - pandocfilters==1.5.0 + - parso==0.8.3 + - pexpect==4.8.0 + - pickleshare==0.7.5 + - prometheus-client==0.14.1 + - psutil==5.9.0 + - ptyprocess==0.7.0 + - pure-eval==0.2.2 + - pycapnp==1.1.0 + - pyopengl==3.1.6 + - pyquaternion==0.9.9 + - pyrealsense2==2.50.0.3812 + - pyrsistent==0.18.1 + - pywavefront==1.3.3 + - pywavelets==1.3.0 + - pyzmq==22.3.0 + - ruamel-yaml==0.17.21 + - ruamel-yaml-clib==0.2.6 + - scikit-image==0.19.2 + - scikit-learn==1.1.0 + - send2trash==1.8.0 + - setproctitle==1.2.3 + - sklearn==0.0 + - sniffio==1.2.0 + - sophuspy==0.0.8 + - soupsieve==2.3.2.post1 + - stack-data==0.2.0 + - sympy==1.10.1 + - terminado==0.15.0 + - threadpoolctl==3.1.0 + - tifffile==2022.5.4 + - tinycss2==1.1.1 + - toml==0.10.2 + - tomlkit==0.10.2 + - traitlets==5.2.1.post0 + - transforms3d==0.3.1 + - trimesh==3.12.0 + - webencodings==0.5.1 + - websocket-client==1.3.2 + - widgetsnbextension==3.6.0 +prefix: /home/yixinlin/miniconda3/envs/polygrasp diff --git a/perception/sandbox/polygrasp/msetup.py b/perception/sandbox/polygrasp/msetup.py new file mode 100644 index 0000000000..b970478baf --- /dev/null +++ b/perception/sandbox/polygrasp/msetup.py @@ -0,0 +1,38 @@ +import mrp + +# For the Conda environments, see README for installation instructions. +# TODO: automatic creation of these Conda environments. + +mrp.process( + name="segmentation_server", + runtime=mrp.Conda( + run_command=["python", "-m", "utils.mrp_wrapper"], + use_named_env="unseen-object-clustering", + ), +) + +mrp.process( + name="grasp_server", + runtime=mrp.Conda( + run_command=["python", "-m", "graspnet_baseline.mrp_wrapper"], + use_named_env="graspnet-baseline", + ), +) + +mrp.process( + name="cam_pub", + runtime=mrp.Conda( + use_named_env="polygrasp", + run_command=["python", "-m", "polygrasp.cam_pub_sub"], + ), +) + +mrp.process( + name="gripper_server", + runtime=mrp.Conda( + use_named_env="polygrasp", + run_command=["launch_gripper.py", "gripper=robotiq_2f", "gripper.comport=/dev/ttyUSB1"], + ), +) + +mrp.main() diff --git a/perception/sandbox/polygrasp/pyproject.toml b/perception/sandbox/polygrasp/pyproject.toml new file mode 100644 index 0000000000..9ca4f69f61 --- /dev/null +++ b/perception/sandbox/polygrasp/pyproject.toml @@ -0,0 +1,19 @@ +[tool.black] +line-length = 88 +target-version = ['py37', 'py38'] +include = '\.pyi?$' +exclude = ''' +/( + \.eggs + | \.git + | \.hg + | \.mypy_cache + | \.tox + | \.venv + | _build + | buck-out + | build + | dist + | third_party +)/ +''' diff --git a/perception/sandbox/polygrasp/scripts/ar_tag_pos.py b/perception/sandbox/polygrasp/scripts/ar_tag_pos.py new file mode 100644 index 0000000000..a4085fbc8d --- /dev/null +++ b/perception/sandbox/polygrasp/scripts/ar_tag_pos.py @@ -0,0 +1,470 @@ +#!/usr/bin/env python +""" +Connects to robot, camera(s), gripper, and grasp server. +Runs grasps generated from grasp server. +""" + + +#old +# high_position_open= torch.tensor([-2.8628, 1.7270, 1.8333, -1.6115, -1.6032, 1.4454, 0.4743]) +# top closed1 :tensor([-2.6614, 1.7002, 1.3380, -1.7611, -1.6574, 1.8371, 0.7491]) +# top closed2 : tensor([-2.4981, 1.7320, 1.0551, -1.6264, -1.6605, 2.1295, 0.7341]) +# bottom closed1: tensor([-2.5299, 1.6819, 0.8728, -1.4600, -1.5670, 2.3802, -2.7360]) +# bottom closed2: tensor([-2.3284, 1.7304, 0.6423, -1.2444, -1.4370, 2.5559, -2.7209]) +# top open: tensor([-2.7492, 1.7202, 1.0310, -1.1538, -1.3008, 2.1873, -2.8715]) +# bottom open: tensor([-2.4589, 1.7582, 0.5844, -0.9734, -1.1897, 2.4049, -2.8648]) +drawer_camera_index = 1 +top_rack_ar_tag = [26, 22, 27] +bottom_rack_ar_tag = [23,29] + +import time +import os + +import numpy as np +from perception.sandbox.polygrasp.scripts.run_grasp import drop_object_in_drawer +import sklearn +import matplotlib.pyplot as plt + +import torch +import hydra +import omegaconf + +import json +import open3d as o3d +from pathlib import Path +from scipy.stats import mode +from functools import partial + +import polygrasp +from polygrasp.segmentation_rpc import SegmentationClient +from polygrasp.grasp_rpc import GraspClient +from polygrasp.serdes import load_bw_img + +import fairotag + +# top_rack_open_ar_tag = [-0.05914402, -0.60028556, -0.38374834] + +top_closed_1 = torch.tensor([-2.8709, 1.7132, 1.3774, -1.8681, -1.4531, 1.9806, 0.4803]) +top_closed_2 = torch.tensor([-2.5949, 1.7388, 1.0075, -1.6537, -1.4691, 2.3417, 0.4605]) +top_open = torch.tensor([-2.8362, 1.7326, 1.0338, -1.2461, -1.4473, 2.2300, -0.0111]) +bottom_closed_1 = torch.tensor([-2.7429, 1.7291, 1.0249, -1.3325, -1.0166, 2.1604, -0.1720]) +bottom_closed_2 = torch.tensor([-2.2719, 1.675, 0.6623, -1.1511, -0.6550, 2.4067, -0.1707]) +bottom_open = torch.tensor([-2.2077, 1.65, 0.2767, -0.7902, -0.6421, 2.5545, -0.2362]) +high_position_close = torch.tensor([-2.8740, 1.3173, 1.5164, -1.2091, -1.1478, 1.4974, -0.1642]) + +def move_to_joint_pos(robot, pos, time_to_go=5.0): + state_log = [] + while len(state_log) < time_to_go*100: + state_log = robot.move_to_joint_positions(pos, time_to_go) + return state_log + + +def open_bottom_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, bottom_closed_1) + traj = move_to_joint_pos(robot, bottom_closed_2) + traj = move_to_joint_pos(robot, bottom_open) + traj = move_to_joint_pos(robot, high_position_close) + +def open_top_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, top_closed_1) + traj = move_to_joint_pos(robot, top_closed_2) + traj = move_to_joint_pos(robot, top_open) + traj = move_to_joint_pos(robot, high_position_close) + +def close_top_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, top_open) + traj = move_to_joint_pos(robot, top_closed_2) + traj = move_to_joint_pos(robot, top_closed_1) + traj = move_to_joint_pos(robot, high_position_close) + +def close_bottom_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, bottom_open) + traj = move_to_joint_pos(robot, bottom_closed_2) + traj = move_to_joint_pos(robot, bottom_closed_1) + traj = move_to_joint_pos(robot, high_position_close) + +def view_pcd(pcd): + o3d.visualization.draw_geometries([pcd]) + return + + +def get_object_detect_dict(obj_pcds): + data = {} + for idx, pcd in enumerate(obj_pcds): + data[idx] = mode(pcd.colors, axis=0).mode.tolist() + return data + + +# only to record the rgb mode values and manually label +def save_object_detect_dict(obj_pcds): + data = get_object_detect_dict(obj_pcds) + with open('category_to_rgb_mapping.json', 'w') as f: + json.dump(data, f, indent=4) + return + + +def read_object_detect_dict(pathname): + with open(pathname, 'r') as f: + data = json.load(f) + return data + +def is_shadow(val, epsilon=5e-2): + err_01 = abs(val[0] - val[1]) + err_21 = abs(val[2] - val[1]) + err_02 = abs(val[2] - val[0]) + if err_01 < epsilon and err_21 < epsilon and err_02 < epsilon: + return True + return False + + +def get_category_to_pcd_map(obj_pcds, cur_data, ref_data): + category_to_pcd_map = {} + for idx, val in cur_data.items(): + min_err = 1000 + min_err_category = 'ignore' # default + if is_shadow(val[0]): + continue + for category, rgb_value in ref_data.items(): + err = np.linalg.norm(np.array(val) - np.array(rgb_value)) + if min_err > err: + min_err = err + min_err_category = category + category_to_pcd_map[min_err_category] = obj_pcds[idx] + return category_to_pcd_map + + +def save_rgbd_masked(rgbd, rgbd_masked): + num_cams = rgbd.shape[0] + f, axarr = plt.subplots(2, num_cams) + + for i in range(num_cams): + if num_cams > 1: + ax1, ax2 = axarr[0, i], axarr[1, i] + else: + ax1, ax2 = axarr + ax1.imshow(rgbd[i, :, :, :3].astype(np.uint8)) + ax2.imshow(rgbd_masked[i, :, :, :3].astype(np.uint8)) + + f.savefig("rgbd_masked.png") + plt.close(f) + + +def get_obj_grasps(grasp_client, obj_pcds, scene_pcd): + for obj_i, obj_pcd in enumerate(obj_pcds): + print(f"Getting obj {obj_i} grasp...") + grasp_group = grasp_client.get_grasps(obj_pcd) + filtered_grasp_group = grasp_client.get_collision(grasp_group, scene_pcd) + if len(filtered_grasp_group) < len(grasp_group): + print( + "Filtered" + f" {len(grasp_group) - len(filtered_grasp_group)}/{len(grasp_group)} grasps" + " due to collision." + ) + if len(filtered_grasp_group) > 0: + return obj_i, filtered_grasp_group + raise Exception( + "Unable to find any grasps after filtering, for any of the" + f" {len(obj_pcds)} objects" + ) + + +def merge_pcds(pcds, eps=0.1, min_samples=2): + """Cluster object pointclouds from different cameras based on centroid using DBSCAN; merge when within eps""" + xys = np.array([pcd.get_center()[:2] for pcd in pcds]) + cluster_labels = ( + sklearn.cluster.DBSCAN(eps=eps, min_samples=min_samples).fit(xys).labels_ + ) + + # Logging + total_n_objs = len(xys) + total_clusters = cluster_labels.max() + 1 + unclustered_objs = (cluster_labels < 0).sum() + print( + f"Clustering objects from all cameras: {total_clusters} clusters, plus" + f" {unclustered_objs} non-clustered objects; went from {total_n_objs} to" + f" {total_clusters + unclustered_objs} objects" + ) + + # Cluster label == -1 when unclustered, otherwise cluster label >=0 + final_pcds = [] + cluster_to_pcd = dict() + for cluster_label, pcd in zip(cluster_labels, pcds): + if cluster_label >= 0: + if cluster_label not in cluster_to_pcd: + cluster_to_pcd[cluster_label] = pcd + else: + cluster_to_pcd[cluster_label] += pcd + else: + final_pcds.append(pcd) + + return list(cluster_to_pcd.values()) + final_pcds + + +def execute_grasp(robot, chosen_grasp, hori_offset, time_to_go): + traj, success = robot.grasp( + chosen_grasp, time_to_go=time_to_go, gripper_width_success_threshold=0.001 + ) + print(f"Grasp success: {success}") + breakpoint() + if success: + print("Placing object in hand to desired pose...") + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0, 0.3]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + print('Move towards drawer to release') + drop_object_in_drawer(robot) + # print("Moving horizontally") + # traj += robot.move_until_success( + # position=torch.Tensor([-0.09, -0.61, 0.2]), + # orientation=[1,0,0,0], + # time_to_go=time_to_go, + # ) + # curr_pose, curr_ori = robot.get_ee_pose() + # print("Moving down") + # traj += robot.move_until_success( + # position=curr_pose + torch.Tensor([0, 0.0, -0.20]), + # orientation=[1,0,0,0], + # time_to_go=time_to_go, + # ) + + print("Opening gripper") + robot.gripper_open() + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0.0, 0.3]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + + return traj + +def pickplace( + robot, + category_order, + cfg, + masks_1, + masks_2, + root_working_dir, + cameras, + frt_cams, + segmentation_client, + grasp_client, +): + for outer_i in range(cfg.num_bin_shifts): + cam_i = outer_i % 2 + print(f"=== Starting bin shift with cam {cam_i} ===") + + # Define some parameters for each workspace. + if cam_i == 0: + masks = masks_1 + hori_offset = torch.Tensor([0, -0.4, 0]) + else: + masks = masks_2 + hori_offset = torch.Tensor([0, 0.4, 0]) + time_to_go = 5 + + for i in range(cfg.num_grasps_per_bin_shift): + # Create directory for current grasp iteration + os.chdir(root_working_dir) + timestamp = int(time.time()) + os.makedirs(f"{timestamp}") + os.chdir(f"{timestamp}") + + print( + f"=== Grasp {i + 1}/{cfg.num_grasps_per_bin_shift}, logging to" + f" {os.getcwd()} ===" + ) + + print("Getting rgbd and pcds..") + rgbd = cameras.get_rgbd() + rgbd_masked = rgbd + + print("Detecting markers & their corresponding points") + uint_rgbs = rgbd[:,:,:,:3].astype(np.uint8) + id_to_pose = {} + for i, (frt, uint_rgb) in enumerate(zip(frt_cams, uint_rgbs)): + markers = frt.detect_markers(uint_rgb) + for marker in markers: + w_i, h_i = marker.corner.mean(axis=0).round().astype(int) + + single_point = np.zeros_like(rgbd[i]) + single_point[h_i, w_i, :] = rgbd[i, h_i, w_i, :] + pcd = cameras.get_pcd_i(single_point, i) + xyz = pcd.get_center() + id_to_pose[marker.id] = xyz + + scene_pcd = cameras.get_pcd(rgbd) + save_rgbd_masked(rgbd, rgbd_masked) + + print("Segmenting image...") + unmerged_obj_pcds = [] + for i in range(cameras.n_cams): + obj_masked_rgbds, obj_masks = segmentation_client.segment_img( + rgbd_masked[i], min_mask_size=cfg.min_mask_size + ) + unmerged_obj_pcds += [ + cameras.get_pcd_i(obj_masked_rgbd, i) + for obj_masked_rgbd in obj_masked_rgbds + ] + + obj_pcds = merge_pcds(unmerged_obj_pcds) + # breakpoint() + + + if len(obj_pcds) == 0: + print( + f"Failed to find any objects with mask size > {cfg.min_mask_size}!" + ) + return 1 + + # print(f"Finding ARTag") + # id_to_pcd = {} + # obj_centers = np.array([x.get_center() for x in obj_pcds]) + # for id, pose in id_to_pose.items(): + # dists = np.linalg.norm(obj_centers - pose, axis=1) + # argmin = dists.argmin() + # min = dists[argmin] + # if min < 0.05: + # id_to_pcd[id] = obj_pcds[argmin] + # print(f"Found object pointclouds corresponding to ARTag ids {list(id_to_pcd.keys())}") + cur_data = get_object_detect_dict(obj_pcds) + ref_data = read_object_detect_dict(pathname=Path( + hydra.utils.get_original_cwd(), + 'data', + 'category_to_rgb_map.json' + ).as_posix()) + + # breakpoint() + category_to_pcd_map = get_category_to_pcd_map(obj_pcds, cur_data, ref_data) + for _cat in category_order: + if _cat not in category_to_pcd_map.keys(): + continue + else: + break + # _cat = category_order[idx] + _pcd = category_to_pcd_map.get(_cat, None) + if _pcd is None: + # break #point() + return 2 + # replace obj_pcds with id_to_pcd[id] for grasping selected id + # for _cat, _pcd in category_to_pcd_map.items(): + print(f'Grasping {_cat}') + + # print("Getting grasps per object...") + obj_i, filtered_grasp_group = get_obj_grasps( + grasp_client, [_pcd], scene_pcd + ) + + # print("Choosing a grasp for the object") + chosen_grasp, final_filtered_grasps = robot.select_grasp( + filtered_grasp_group + ) + grasp_client.visualize_grasp(scene_pcd, final_filtered_grasps) + grasp_client.visualize_grasp( + _pcd, final_filtered_grasps, name=_cat + ) + + traj = execute_grasp(robot, chosen_grasp, hori_offset, time_to_go) + + print("Going home") + robot.go_home() + +def get_marker_corners(cameras, frt_cams, root_working_dir, name): + rgbd = cameras.get_rgbd() + rgbd_masked = rgbd + save_rgbd_masked(rgbd, rgbd_masked) + uint_rgbs = rgbd[:,:,:,:3].astype(np.uint8) + drawer_markers = frt_cams[drawer_camera_index].detect_markers(uint_rgbs[drawer_camera_index]) + print(drawer_markers) + data = { + name : [{ + "id": int(m.id), + "corner": m.corner.astype(np.int32).tolist() + } for m in drawer_markers] + } + with open(Path(root_working_dir, 'data', name + '.json').as_posix(), 'w') as f: + json.dump(data, f, indent=2) + return + +@hydra.main(config_path="../conf", config_name="run_grasp") +def main(cfg): + root_working_dir = hydra.utils.get_original_cwd() # os.getcwd() + print(f"Config:\n{omegaconf.OmegaConf.to_yaml(cfg, resolve=True)}") + print(f"Current working directory: {os.getcwd()}") + + print("Initialize robot & gripper") + robot = hydra.utils.instantiate(cfg.robot) + robot.gripper_open() + robot.go_home() + # robot.move_until_success(position=torch.tensor([ 0.70, -0.07, 0.0101]), orientation=robot.get_ee_pose()[1], time_to_go=10) + + print("Initializing cameras") + cfg.cam.intrinsics_file = hydra.utils.to_absolute_path(cfg.cam.intrinsics_file) + cfg.cam.extrinsics_file = hydra.utils.to_absolute_path(cfg.cam.extrinsics_file) + cameras = hydra.utils.instantiate(cfg.cam) + + # print("Loading camera workspace masks") + # # import pdb; pdb.set_trace() + # masks_1 = np.array( + # [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_1], + # dtype=np.float64, + # ) + # masks_1[-1,:,:] *=0 + # masks_2 = np.array( + # [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_2], + # dtype=np.float64, + # ) + # masks_1[:2,:,:] *=0 + + print("Connect to grasp candidate selection and pointcloud processor") + segmentation_client = SegmentationClient() + grasp_client = GraspClient( + view_json_path=hydra.utils.to_absolute_path(cfg.view_json_path) + ) + + print("Loading ARTag modules") + frt_cams = [fairotag.CameraModule() for _ in range(cameras.n_cams)] + for frt, intrinsics in zip(frt_cams, cameras.intrinsics): + frt.set_intrinsics(intrinsics) + MARKER_LENGTH = 0.04 + MARKER_ID = [27, 26, 23, 29] + for i in MARKER_ID: + for frt in frt_cams: + frt.register_marker_size(i, MARKER_LENGTH) + + get_marker_corners(cameras, frt_cams, root_working_dir, name='all_closed') + breakpoint() + + open_bottom_drawer(robot) + robot.go_home() + breakpoint() + + get_marker_corners(cameras, frt_cams, root_working_dir, name='open_bottom_drawer') + + open_top_drawer(robot) + robot.go_home() + breakpoint() + get_marker_corners(cameras, frt_cams, root_working_dir, name='all_open') + + close_top_drawer(robot) + close_bottom_drawer(robot) + open_top_drawer(robot) + robot.go_home() + breakpoint() + get_marker_corners(cameras, frt_cams, root_working_dir, name='open_top_drawer') + + close_bottom_drawer(robot) + robot.go_home() + breakpoint() + get_marker_corners(cameras, frt_cams, root_working_dir, name='all_closed') +if __name__ == "__main__": + main() diff --git a/perception/sandbox/polygrasp/scripts/object_segment.py b/perception/sandbox/polygrasp/scripts/object_segment.py new file mode 100644 index 0000000000..ded15885fc --- /dev/null +++ b/perception/sandbox/polygrasp/scripts/object_segment.py @@ -0,0 +1,451 @@ +#!/usr/bin/env python +""" +Connects to robot, camera(s), gripper, and grasp server. +Runs grasps generated from grasp server. +""" + + +#old +# high_position_open= torch.tensor([-2.8628, 1.7270, 1.8333, -1.6115, -1.6032, 1.4454, 0.4743]) +# top closed1 :tensor([-2.6614, 1.7002, 1.3380, -1.7611, -1.6574, 1.8371, 0.7491]) +# top closed2 : tensor([-2.4981, 1.7320, 1.0551, -1.6264, -1.6605, 2.1295, 0.7341]) +# bottom closed1: tensor([-2.5299, 1.6819, 0.8728, -1.4600, -1.5670, 2.3802, -2.7360]) +# bottom closed2: tensor([-2.3284, 1.7304, 0.6423, -1.2444, -1.4370, 2.5559, -2.7209]) +# top open: tensor([-2.7492, 1.7202, 1.0310, -1.1538, -1.3008, 2.1873, -2.8715]) +# bottom open: tensor([-2.4589, 1.7582, 0.5844, -0.9734, -1.1897, 2.4049, -2.8648]) + + + +import time +import os + +import numpy as np +import sklearn +import matplotlib.pyplot as plt + +import torch +import hydra +import omegaconf + +import json +import open3d as o3d +from pathlib import Path +from scipy.stats import mode +from functools import partial + +import polygrasp +from polygrasp.segmentation_rpc import SegmentationClient +from polygrasp.grasp_rpc import GraspClient +from polygrasp.serdes import load_bw_img + +import fairotag + +top_closed_1 = torch.tensor([-2.8709, 1.7132, 1.3774, -1.8681, -1.4531, 1.9806, 0.4803]) +top_closed_2 = torch.tensor([-2.5949, 1.7388, 1.0075, -1.6537, -1.4691, 2.3417, 0.4605]) +top_open = torch.tensor([-2.8362, 1.7326, 1.0338, -1.2461, -1.4473, 2.2300, -0.0111]) +bottom_closed_1 = torch.tensor([-2.7429, 1.7291, 1.0249, -1.3325, -1.0166, 2.1604, -0.1720]) +bottom_closed_2 = torch.tensor([-2.2719, 1.675, 0.6623, -1.1511, -0.6550, 2.4067, -0.1707]) +bottom_open = torch.tensor([-2.2077, 1.65, 0.2767, -0.7902, -0.6421, 2.5545, -0.2362]) +high_position_close = torch.tensor([-2.8740, 1.3173, 1.5164, -1.2091, -1.1478, 1.4974, -0.1642]) + +def move_to_joint_pos(robot, pos, time_to_go=5.0): + state_log = [] + while len(state_log) < time_to_go*100: + state_log = robot.move_to_joint_positions(pos, time_to_go) + return state_log + + +def open_bottom_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, bottom_closed_1) + traj = move_to_joint_pos(robot, bottom_closed_2) + traj = move_to_joint_pos(robot, bottom_open) + traj = move_to_joint_pos(robot, high_position_close) + +def open_top_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, top_closed_1) + traj = move_to_joint_pos(robot, top_closed_2) + traj = move_to_joint_pos(robot, top_open) + traj = move_to_joint_pos(robot, high_position_close) + +def close_top_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, top_open) + traj = move_to_joint_pos(robot, top_closed_2) + traj = move_to_joint_pos(robot, top_closed_1) + traj = move_to_joint_pos(robot, high_position_close) + +def close_bottom_drawer(robot): + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, bottom_open) + traj = move_to_joint_pos(robot, bottom_closed_2) + traj = move_to_joint_pos(robot, bottom_closed_1) + traj = move_to_joint_pos(robot, high_position_close) + +def view_pcd(pcd): + o3d.visualization.draw_geometries([pcd]) + return + + +def get_object_detect_dict(obj_pcds): + data = {} + for idx, pcd in enumerate(obj_pcds): + data[idx] = mode(pcd.colors, axis=0).mode.tolist() + return data + + +# only to record the rgb mode values and manually label +def save_object_detect_dict(obj_pcds): + data = get_object_detect_dict(obj_pcds) + with open('category_to_rgb_mapping.json', 'w') as f: + json.dump(data, f, indent=4) + return + + +def read_object_detect_dict(pathname): + with open(pathname, 'r') as f: + data = json.load(f) + return data + +def is_shadow(val, epsilon=5e-2): + err_01 = abs(val[0] - val[1]) + err_21 = abs(val[2] - val[1]) + err_02 = abs(val[2] - val[0]) + if err_01 < epsilon and err_21 < epsilon and err_02 < epsilon: + return True + return False + + +def get_category_to_pcd_map(obj_pcds, cur_data, ref_data): + category_to_pcd_map = {} + for idx, val in cur_data.items(): + min_err = 1000 + min_err_category = 'ignore' # default + if is_shadow(val[0]): + continue + for category, rgb_value in ref_data.items(): + err = np.linalg.norm(np.array(val) - np.array(rgb_value)) + if min_err > err: + min_err = err + min_err_category = category + category_to_pcd_map[min_err_category] = obj_pcds[idx] + return category_to_pcd_map + + +def save_rgbd_masked(rgbd, rgbd_masked): + num_cams = rgbd.shape[0] + f, axarr = plt.subplots(2, num_cams) + + for i in range(num_cams): + if num_cams > 1: + ax1, ax2 = axarr[0, i], axarr[1, i] + else: + ax1, ax2 = axarr + ax1.imshow(rgbd[i, :, :, :3].astype(np.uint8)) + ax2.imshow(rgbd_masked[i, :, :, :3].astype(np.uint8)) + + f.savefig("rgbd_masked.png") + plt.close(f) + + +def get_obj_grasps(grasp_client, obj_pcds, scene_pcd): + for obj_i, obj_pcd in enumerate(obj_pcds): + print(f"Getting obj {obj_i} grasp...") + grasp_group = grasp_client.get_grasps(obj_pcd) + filtered_grasp_group = grasp_client.get_collision(grasp_group, scene_pcd) + if len(filtered_grasp_group) < len(grasp_group): + print( + "Filtered" + f" {len(grasp_group) - len(filtered_grasp_group)}/{len(grasp_group)} grasps" + " due to collision." + ) + if len(filtered_grasp_group) > 0: + return obj_i, filtered_grasp_group + raise Exception( + "Unable to find any grasps after filtering, for any of the" + f" {len(obj_pcds)} objects" + ) + + +def merge_pcds(pcds, eps=0.1, min_samples=2): + """Cluster object pointclouds from different cameras based on centroid using DBSCAN; merge when within eps""" + xys = np.array([pcd.get_center()[:2] for pcd in pcds]) + cluster_labels = ( + sklearn.cluster.DBSCAN(eps=eps, min_samples=min_samples).fit(xys).labels_ + ) + + # Logging + total_n_objs = len(xys) + total_clusters = cluster_labels.max() + 1 + unclustered_objs = (cluster_labels < 0).sum() + print( + f"Clustering objects from all cameras: {total_clusters} clusters, plus" + f" {unclustered_objs} non-clustered objects; went from {total_n_objs} to" + f" {total_clusters + unclustered_objs} objects" + ) + + # Cluster label == -1 when unclustered, otherwise cluster label >=0 + final_pcds = [] + cluster_to_pcd = dict() + for cluster_label, pcd in zip(cluster_labels, pcds): + if cluster_label >= 0: + if cluster_label not in cluster_to_pcd: + cluster_to_pcd[cluster_label] = pcd + else: + cluster_to_pcd[cluster_label] += pcd + else: + final_pcds.append(pcd) + + return list(cluster_to_pcd.values()) + final_pcds + + +def execute_grasp(robot, chosen_grasp, hori_offset, time_to_go): + traj, s = robot.grasp( + chosen_grasp, time_to_go=time_to_go, gripper_width_success_threshold=0.001 + ) + print(f"Grasp success: {s}") + breakpoint() + if s: + print("Placing object in hand to desired pose...") + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0, 0.3]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + print("Moving horizontally") + traj += robot.move_until_success( + position=torch.Tensor([-0.09, -0.61, 0.2]), + orientation=[1,0,0,0], + time_to_go=time_to_go, + ) + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving down") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0.0, -0.20]), + orientation=[1,0,0,0], + time_to_go=time_to_go, + ) + + print("Opening gripper") + robot.gripper_open() + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0.0, 0.3]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + + return traj + +def pickplace( + robot, + category_order, + cfg, + masks_1, + masks_2, + root_working_dir, + cameras, + frt_cams, + segmentation_client, + grasp_client, +): + for outer_i in range(cfg.num_bin_shifts): + cam_i = outer_i % 2 + print(f"=== Starting bin shift with cam {cam_i} ===") + + # Define some parameters for each workspace. + if cam_i == 0: + masks = masks_1 + hori_offset = torch.Tensor([0, -0.4, 0]) + else: + masks = masks_2 + hori_offset = torch.Tensor([0, 0.4, 0]) + time_to_go = 5 + + for i in range(cfg.num_grasps_per_bin_shift): + # Create directory for current grasp iteration + os.chdir(root_working_dir) + timestamp = int(time.time()) + os.makedirs(f"{timestamp}") + os.chdir(f"{timestamp}") + + print( + f"=== Grasp {i + 1}/{cfg.num_grasps_per_bin_shift}, logging to" + f" {os.getcwd()} ===" + ) + + print("Getting rgbd and pcds..") + rgbd = cameras.get_rgbd() + rgbd_masked = rgbd + + print("Detecting markers & their corresponding points") + uint_rgbs = rgbd[:,:,:,:3].astype(np.uint8) + id_to_pose = {} + for i, (frt, uint_rgb) in enumerate(zip(frt_cams, uint_rgbs)): + markers = frt.detect_markers(uint_rgb) + for marker in markers: + w_i, h_i = marker.corner.mean(axis=0).round().astype(int) + + single_point = np.zeros_like(rgbd[i]) + single_point[h_i, w_i, :] = rgbd[i, h_i, w_i, :] + pcd = cameras.get_pcd_i(single_point, i) + xyz = pcd.get_center() + id_to_pose[marker.id] = xyz + + scene_pcd = cameras.get_pcd(rgbd) + save_rgbd_masked(rgbd, rgbd_masked) + + print("Segmenting image...") + unmerged_obj_pcds = [] + for i in range(cameras.n_cams): + obj_masked_rgbds, obj_masks = segmentation_client.segment_img( + rgbd_masked[i], min_mask_size=cfg.min_mask_size + ) + unmerged_obj_pcds += [ + cameras.get_pcd_i(obj_masked_rgbd, i) + for obj_masked_rgbd in obj_masked_rgbds + ] + + obj_pcds = merge_pcds(unmerged_obj_pcds) + # breakpoint() + + + if len(obj_pcds) == 0: + print( + f"Failed to find any objects with mask size > {cfg.min_mask_size}!" + ) + return 1 + + # print(f"Finding ARTag") + # id_to_pcd = {} + # obj_centers = np.array([x.get_center() for x in obj_pcds]) + # for id, pose in id_to_pose.items(): + # dists = np.linalg.norm(obj_centers - pose, axis=1) + # argmin = dists.argmin() + # min = dists[argmin] + # if min < 0.05: + # id_to_pcd[id] = obj_pcds[argmin] + # print(f"Found object pointclouds corresponding to ARTag ids {list(id_to_pcd.keys())}") + cur_data = get_object_detect_dict(obj_pcds) + ref_data = read_object_detect_dict(pathname=Path( + hydra.utils.get_original_cwd(), + 'data', + 'category_to_rgb_map.json' + ).as_posix()) + + # breakpoint() + category_to_pcd_map = get_category_to_pcd_map(obj_pcds, cur_data, ref_data) + for _cat in category_order: + if _cat not in category_to_pcd_map.keys(): + continue + else: + break + # _cat = category_order[idx] + _pcd = category_to_pcd_map.get(_cat, None) + if _pcd is None: + # break #point() + return 2 + # replace obj_pcds with id_to_pcd[id] for grasping selected id + # for _cat, _pcd in category_to_pcd_map.items(): + print(f'Grasping {_cat}') + breakpoint() + # print("Getting grasps per object...") + obj_i, filtered_grasp_group = get_obj_grasps( + grasp_client, [_pcd], scene_pcd + ) + + # print("Choosing a grasp for the object") + chosen_grasp, final_filtered_grasps = robot.select_grasp( + filtered_grasp_group + ) + grasp_client.visualize_grasp(scene_pcd, final_filtered_grasps) + grasp_client.visualize_grasp( + _pcd, final_filtered_grasps, name=_cat + ) + + traj = execute_grasp(robot, chosen_grasp, hori_offset, time_to_go) + + print("Going home") + robot.go_home() + +@hydra.main(config_path="../conf", config_name="run_grasp") +def main(cfg): + print(f"Config:\n{omegaconf.OmegaConf.to_yaml(cfg, resolve=True)}") + print(f"Current working directory: {os.getcwd()}") + + print("Initializing cameras") + cfg.cam.intrinsics_file = hydra.utils.to_absolute_path(cfg.cam.intrinsics_file) + cfg.cam.extrinsics_file = hydra.utils.to_absolute_path(cfg.cam.extrinsics_file) + cameras = hydra.utils.instantiate(cfg.cam) + + # breakpoint() + # print("Loading camera workspace masks") + # import pdb; pdb.set_trace() + # masks_1 = np.array( + # [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_1], + # dtype=np.float64, + # ) + # masks_1[-1,:,:] *=0 + # masks_2 = np.array( + # [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_2], + # dtype=np.float64, + # ) + # masks_1[:2,:,:] *=0 + + print("Connect to grasp candidate selection and pointcloud processor") + segmentation_client = SegmentationClient() + rgbd = cameras.get_rgbd() + scene_pcd = cameras.get_pcd(rgbd) + breakpoint() + # save_rgbd_masked(rgbd) + + # grasp_client = GraspClient( + # view_json_path=hydra.utils.to_absolute_path(cfg.view_json_path) + # ) + + # print("Loading ARTag modules") + # frt_cams = [fairotag.CameraModule() for _ in range(cameras.n_cams)] + # for frt, intrinsics in zip(frt_cams, cameras.intrinsics): + # frt.set_intrinsics(intrinsics) + # MARKER_LENGTH = 0.04 + # MARKER_ID = [18] + # for i in MARKER_ID: + # for frt in frt_cams: + # frt.register_marker_size(i, MARKER_LENGTH) + + unmerged_obj_pcds = [] + for i in range(cameras.n_cams): + + obj_masked_rgbds, obj_masks = segmentation_client.segment_img( + rgbd[i], min_mask_size=cfg.min_mask_size + ) + unmerged_obj_pcds += [ + cameras.get_pcd_i(obj_masked_rgbd, i) + for obj_masked_rgbd in obj_masked_rgbds + ] + + obj_pcds = merge_pcds(unmerged_obj_pcds) + cur_data = get_object_detect_dict(obj_pcds) + ref_data = read_object_detect_dict(pathname=Path( + hydra.utils.get_original_cwd(), + 'data', + 'category_to_rgb_map.json' + ).as_posix()) + + category_to_pcd_map = get_category_to_pcd_map(obj_pcds, cur_data, ref_data) + # saving pcd + # for name, pcd in category_to_pcd_map.items(): + # o3d.io.write_point_cloud(name, pcd, compressed=True) + + hw_points = {name: pcd.compute_convex_hull().get_center().tolist() for name, pcd in category_to_pcd_map.items()} + + with open(hydra.utils.get_original_cwd() + '/data/hw_points_convex_hull_centers.json', 'w') as f: + json.dump(hw_points, f, indent=4) + + +if __name__ == "__main__": + main() diff --git a/perception/sandbox/polygrasp/scripts/run_grasp.py b/perception/sandbox/polygrasp/scripts/run_grasp.py new file mode 100644 index 0000000000..269ed1b499 --- /dev/null +++ b/perception/sandbox/polygrasp/scripts/run_grasp.py @@ -0,0 +1,691 @@ +#!/usr/bin/env python +""" +Connects to robot, camera(s), gripper, and grasp server. +Runs grasps generated from grasp server. +""" + + +#old +# high_position_open= torch.tensor([-2.8628, 1.7270, 1.8333, -1.6115, -1.6032, 1.4454, 0.4743]) +# top closed1 :tensor([-2.6614, 1.7002, 1.3380, -1.7611, -1.6574, 1.8371, 0.7491]) +# top closed2 : tensor([-2.4981, 1.7320, 1.0551, -1.6264, -1.6605, 2.1295, 0.7341]) +# bottom closed1: tensor([-2.5299, 1.6819, 0.8728, -1.4600, -1.5670, 2.3802, -2.7360]) +# bottom closed2: tensor([-2.3284, 1.7304, 0.6423, -1.2444, -1.4370, 2.5559, -2.7209]) +# top open: tensor([-2.7492, 1.7202, 1.0310, -1.1538, -1.3008, 2.1873, -2.8715]) +# bottom open: tensor([-2.4589, 1.7582, 0.5844, -0.9734, -1.1897, 2.4049, -2.8648]) + + + +import time +import os +import cv2 + +import numpy as np +import sklearn +import matplotlib.pyplot as plt + +import torch +import hydra +import omegaconf + +import json +import open3d as o3d +from pathlib import Path +from scipy.stats import mode +from functools import partial + +import polygrasp +from polygrasp.segmentation_rpc import SegmentationClient +from polygrasp.grasp_rpc import GraspClient +from polygrasp.serdes import load_bw_img + +import fairotag + +threshold_no_grasps_from_graspnet_error = 5 +# top_closed_1 = torch.tensor([-2.8709, 1.7132, 1.3774, -1.8681, -1.4531, 1.9806, 0.4803]) +# top_closed_2 = torch.tensor([-2.5949, 1.7388, 1.0075, -1.6537, -1.4691, 2.3417, 0.4605]) +# top_open = torch.tensor([-2.8362, 1.7326, 1.0338, -1.2461, -1.4473, 2.2300, -0.0111]) +# bottom_closed_1 = torch.tensor([-2.7429, 1.7291, 1.0249, -1.3325, -1.0166, 2.1604, -0.1720]) +# bottom_closed_2 = torch.tensor([-2.1674, 1.6435, 0.5143, -1.2161, -0.9969, 2.5138, 0.2471]) +# #torch.tensor([-2.2719, 1.675, 0.6623, -1.1511, -0.6550, 2.4067, -0.1707]) +# bottom_open = torch.tensor([-2.4781, 1.7273, 0.5201, -0.9822, -0.9824, 2.4964, -0.0416]) +# # torch.tensor([-2.2077, 1.65, 0.2767, -0.7902, -0.6421, 2.5545, -0.2362]) +# high_position_close = torch.tensor([-2.8740, 1.3173, 1.5164, -1.2091, -1.1478, 1.4974, -0.1642]) +# sink_pose = torch.tensor([-0.2135, -0.0278, 0.5381, -2.1573, 0.0384, 2.1235, -0.6401]) +# # torch.tensor([-1.1165, 0.7988, 1.5438, -2.3060, -1.0097, 2.0797, -0.5347]) +rise_above_height = 0.3 +top_closed_1 = torch.tensor([-2.8709, 1.7132, 1.3774, -1.3681, -1.4531, 1.9806, 0.4803]) +top_closed_2 = torch.tensor([-2.5949, 1.6388, 1.0075, -1.6537, -1.4691, 2.3417, 0.4605]) +top_open = torch.tensor([-2.8362, 1.7326, 1.0338, -1.2461, -1.4473, 2.2300, -0.0111]) +bottom_closed_1 = torch.tensor([-2.7429, 1.7291, 1.0249, -1.3325, -1.0166, 2.1604, -0.1720]) +bottom_closed_2 = torch.tensor([-2.1674, 1.6435, 0.5143, -1.2161, -0.9969, 2.5138, 0.2471]) +#torch.tensor([-2.2719, 1.675, 0.6623, -1.1511, -0.6550, 2.4067, -0.1707]) +bottom_open = torch.tensor([-2.4781, 1.7273, 0.5201, -0.9822, -0.9824, 2.4964, -0.0416]) +# torch.tensor([-2.2077, 1.65, 0.2767, -0.7902, -0.6421, 2.5545, -0.2362]) +high_position_close = torch.tensor([-2.8740, 1.3173, 1.5164, -1.2091, -1.1478, 1.4974, -0.1642]) +sink_pose = torch.tensor([-0.2135, -0.0278, 0.5381, -2.1573, 0.0384, 2.1235, -0.6401]) +drop_pose = torch.tensor([-1.6796, 0.7708, 0.0031, -1.9066, 0.0218, 2.7228, -1.8208]) +above_the_edge = torch.tensor([-2.0, 0.1228, 0.3624, -1.7097, -0.0876, 1.8700, -1.5221]) + +# torch.tensor([-1.1165, 0.7988, 1.5438, -2.3060, -1.0097, 2.0797, -0.5347]) +kernel = np.ones((3,3), np.uint8) + +def move_to_joint_pos(robot, pos, time_to_go=5.0): + """To handle if robot.move_to_joint_positions does not + reach the desired position in one-go. + Repeat until the state-log length is above some threshold + """ + state_log = [] + while len(state_log) < time_to_go*700: + state_log = robot.move_to_joint_positions(pos, time_to_go) + print("length of state log", len(state_log)) + return state_log + + +def open_bottom_drawer(robot): + print('open_bottom_drawer') + traj = move_to_joint_pos(robot, above_the_edge) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, bottom_closed_1) + traj = move_to_joint_pos(robot, bottom_closed_2) + traj = move_to_joint_pos(robot, bottom_open) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, above_the_edge) + +def open_top_drawer(robot): + print('open_top_drawer') + traj = move_to_joint_pos(robot, above_the_edge) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, top_closed_1) + traj = move_to_joint_pos(robot, top_closed_2) + traj = move_to_joint_pos(robot, top_open) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, above_the_edge) + +def close_top_drawer(robot): + print('close_top_drawer') + traj = move_to_joint_pos(robot, above_the_edge) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, top_open) + traj = move_to_joint_pos(robot, top_closed_2) + traj = move_to_joint_pos(robot, top_closed_1) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, above_the_edge) + +def close_bottom_drawer(robot): + print('close_bottom_drawer') + traj = move_to_joint_pos(robot, above_the_edge) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, bottom_open) + traj = move_to_joint_pos(robot, bottom_closed_2) + traj = move_to_joint_pos(robot, bottom_closed_1) + traj = move_to_joint_pos(robot, high_position_close) + traj = move_to_joint_pos(robot, above_the_edge) + +def drop_object_in_drawer(robot): + print('drop_object_in_drawer') + traj = move_to_joint_pos(robot, above_the_edge) + traj = move_to_joint_pos(robot, drop_pose) + + +def view_pcd(pcd): + o3d.visualization.draw_geometries([pcd]) + return + +def save_depth_images(rgbd): + depth_img = rgbd[:,:,-1] + + + +def get_object_detect_dict(obj_pcds): + data = {} + for idx, pcd in enumerate(obj_pcds): + data[idx] = mode(pcd.colors, axis=0).mode.tolist() + return data + + +# only to record the rgb mode values and manually label +def save_object_detect_dict(obj_pcds): + data = get_object_detect_dict(obj_pcds) + with open('category_to_rgb_mapping.json', 'w') as f: + json.dump(data, f, indent=4) + return + + +def read_object_detect_dict(pathname): + with open(pathname, 'r') as f: + data = json.load(f) + return data + +def is_shadow(val, epsilon=5e-2): + err_01 = abs(val[0] - val[1]) + err_21 = abs(val[2] - val[1]) + err_02 = abs(val[2] - val[0]) + if err_01 < epsilon and err_21 < epsilon and err_02 < epsilon: + return True + return False + + +def is_not_on_counter(xyz): + if xyz[-1] > 0: + return False + return True + + +# def get_category_to_pcd_map(obj_pcds, cur_data, ref_data): +# category_to_pcd_map = {} +# for idx, val in cur_data.items(): +# min_err = 1000 +# min_err_category = 'ignore' # default +# if is_shadow(val[0]): +# continue +# for category, rgb_value in ref_data.items(): +# err = np.linalg.norm(np.array(val) - np.array(rgb_value)) +# if min_err > err: +# min_err = err +# min_err_category = category +# category_to_pcd_map[min_err_category] = obj_pcds[idx] +# return category_to_pcd_map + +def get_category_to_pcd_map(obj_pcds, cur_data, ref_data): + if len(obj_pcds) != len(cur_data): + breakpoint() + category_to_pcd_map = {cat: [] for cat in ref_data} + for idx, val in cur_data.items(): + min_err = 10000 + min_err_category = 'ignore' # default + if is_shadow(val[0]) or is_not_on_counter(obj_pcds[idx].get_center()): + continue + for category, rgb_value in ref_data.items(): + err = np.linalg.norm(np.array(val) - np.array(rgb_value)) + if min_err > err: + min_err = err + min_err_category = category + if min_err_category.startswith('ignore_'): + continue + category_to_pcd_map[min_err_category].append(obj_pcds[idx]) + return category_to_pcd_map + +def save_rgbd_masked(rgbd, rgbd_masked): + num_cams = rgbd.shape[0] + f, axarr = plt.subplots(2, num_cams) + + for i in range(num_cams): + if num_cams > 1: + ax1, ax2 = axarr[0, i], axarr[1, i] + else: + ax1, ax2 = axarr + ax1.imshow(rgbd[i, :, :, :3].astype(np.uint8)) + ax2.imshow(rgbd_masked[i, :, :, :3].astype(np.uint8)) + + f.savefig("rgbd_masked.png") + plt.close(f) + + +def get_obj_grasps(grasp_client, obj_pcds, scene_pcd): + for obj_i, obj_pcd in enumerate(obj_pcds): + print(f"Getting obj {obj_i} grasp...") + # Vidhi: ask Yixin how to visualize these + grasp_group = grasp_client.get_grasps(obj_pcd) + # + # filtered_grasp_group = grasp_client.get_collision(grasp_group, scene_pcd) + # if len(filtered_grasp_group) < len(grasp_group): + # print( + # "Filtered" + # f" {len(grasp_group) - len(filtered_grasp_group)}/{len(grasp_group)} grasps" + # " due to collision." + # ) + # if len(filtered_grasp_group) > 0: + # return obj_i, filtered_grasp_group # filtered_grasp_group + # print( + # "Unable to find any grasps after filtering, for any of the" + # f" {len(obj_pcds)} objects" + # ) + return obj_i, grasp_group # filtered_grasp_group #None, None + + +def merge_pcds(pcds, eps=0.1, min_samples=2): + """Cluster object pointclouds from different cameras based on centroid using DBSCAN; merge when within eps""" + xys = np.array([pcd.get_center()[:2] for pcd in pcds]) + cluster_labels = ( + sklearn.cluster.DBSCAN(eps=eps, min_samples=min_samples).fit(xys).labels_ + ) + + # Logging + total_n_objs = len(xys) + total_clusters = cluster_labels.max() + 1 + unclustered_objs = (cluster_labels < 0).sum() + print( + f"Clustering objects from all cameras: {total_clusters} clusters, plus" + f" {unclustered_objs} non-clustered objects; went from {total_n_objs} to" + f" {total_clusters + unclustered_objs} objects" + ) + + # Cluster label == -1 when unclustered, otherwise cluster label >=0 + final_pcds = [] + cluster_to_pcd = dict() + for cluster_label, pcd in zip(cluster_labels, pcds): + if cluster_label >= 0: + if cluster_label not in cluster_to_pcd: + cluster_to_pcd[cluster_label] = pcd + else: + cluster_to_pcd[cluster_label] += pcd + else: + final_pcds.append(pcd) + + return list(cluster_to_pcd.values()) + final_pcds + + +def execute_grasp(robot, chosen_grasp, time_to_go, place_in_drawer=True) -> bool: + traj, success = robot.grasp( + chosen_grasp, time_to_go=time_to_go, gripper_width_success_threshold=0.001 + ) + print(f"Grasp success: {success}") + + if success: + ## place in sink + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0, rise_above_height]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + curr_pose, curr_ori = robot.get_ee_pose() + + if place_in_drawer: + # print("Placing object in hand to desired pose...") + # traj += robot.move_until_success( + # position=curr_pose + torch.Tensor([-curr_pose[0], 0, 0]), + # orientation=curr_ori, + # time_to_go=time_to_go, + # ) + print("Moving above the drawer") + drop_object_in_drawer(robot) + # traj += robot.move_until_success( + # position=torch.Tensor([-0.09, -0.61, curr_pose[2]]), + # orientation=[1,0,0,0], + # time_to_go=time_to_go, + # ) + # curr_pose, curr_ori = robot.get_ee_pose() + # print("Moving down") + # traj += robot.move_until_success( + # position=curr_pose + torch.Tensor([0, 0.0, -rise_above_height]), + # orientation=[1,0,0,0], + # time_to_go=time_to_go, + # ) + else: + print("Moving to sink pose") + traj += move_to_joint_pos(robot, sink_pose) + + print("Opening gripper") + robot.gripper_open() + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0.0, rise_above_height]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + robot.go_home() + return success + + +def save_rgbd(rgbd): + num_cams = rgbd.shape[0] + f, axarr = plt.subplots(2, num_cams) + + for i in range(num_cams): + if num_cams > 1: + ax1, ax2 = axarr[0, i], axarr[1, i] + else: + ax1, ax2 = axarr + ax1.imshow(rgbd[i, :, :, :3].astype(np.uint8)) + ax2.matshow(rgbd[i, :, :, 3:]) + + f.savefig("rgbd.png") + plt.close(f) + + +# def is_pcd_good(_pcd): +# pcd_extent = (_pcd.get_max_bound() - _pcd.get_min_bound())/2 +# pcd_mode = mode(_pcd.points, axis=0) + +# if abs(pcd_extent - pcd_mode): +# +# return True + + +def pickplace( + robot, + category_order, + cfg, + masks_1, + masks_2, + root_working_dir, + cameras, + frt_cams, + segmentation_client, + grasp_client, +): + no_grasps_from_graspnet_counter = 0 + for outer_i in range(cfg.num_bin_shifts): + cam_i = outer_i % 2 + print(f"=== Starting bin shift with cam {cam_i} ===") + + # Define some parameters for each workspace. + if cam_i == 0: + masks = masks_1 + hori_offset = torch.Tensor([0, -0.4, 0]) + else: + masks = masks_2 + hori_offset = torch.Tensor([0, 0.4, 0]) + time_to_go = 5 + + for i in range(cfg.num_grasps_per_bin_shift): + # Create directory for current grasp iteration + os.chdir(root_working_dir) + timestamp = int(time.time()) + os.makedirs(f"{timestamp}") + os.chdir(f"{timestamp}") + + print( + f"=== Grasp {i + 1}/{cfg.num_grasps_per_bin_shift}, logging to" + f" {os.getcwd()} ===" + ) + + print("Getting rgbd and pcds..") + rgbd = cameras.get_rgbd() + print("Detecting markers & their corresponding points") + rgbd_masked = rgbd + + uint_rgbs= rgbd[:,:,:,:3].astype(np.uint8) + id_to_pose = {} + for i, (frt, uint_rgb) in enumerate(zip(frt_cams, uint_rgbs)): + markers = frt.detect_markers(uint_rgb) + for marker in markers: + w_i, h_i = marker.corner.mean(axis=0).round().astype(int) + + single_point = np.zeros_like(rgbd[i]) + single_point[h_i, w_i, :] = rgbd[i, h_i, w_i, :] + pcd = cameras.get_pcd_i(single_point, i) + xyz = pcd.get_center() + id_to_pose[marker.id] = xyz + + scene_pcd = cameras.get_pcd(rgbd) + # + # source_pcd = cameras.get_pcd(rgbd[0]) + + save_rgbd_masked(rgbd, rgbd_masked) + + print("Segmenting image...") + unmerged_obj_pcds = [] + for cam_count in range(cameras.n_cams): + obj_masked_rgbds, obj_masks = segmentation_client.segment_img( + rgbd_masked[cam_count], min_mask_size=cfg.min_mask_size + ) + # check masked obj rgbd open/close + # depth_map = obj_masked_rgbds[0][:, :, 3] + # max_depth = depth_map.max() + # normalized_depth_map = depth_map/max_depth + # # img = cv2.cvtColor(normalized_depth_map, cv2.COLOR_GRAY2BGR) + # img_open = cv2.morphologyEx(img, cv2.MORPH_OPEN, kernel) + # img_close = cv2.morphologyEx(img_open, cv2.MORPH_CLOSE, kernel) + + + # Erode the masks to get clean rgb-depth alignment + for j in range(len(obj_masks)): + mask_3channels = np.stack([obj_masks[j]]*3, -1) + mask_3channels = mask_3channels.astype(np.uint8) + mask_cv = cv2.cvtColor(mask_3channels, cv2.COLOR_BGR2RGB) + mask_erode = cv2.erode(mask_cv, kernel, iterations=5) + _pcd = cameras.get_pcd_i(obj_masked_rgbds[j]*mask_erode[:,:,:1], cam_count) + # DEBUG + unmerged_obj_pcds.append(_pcd) + # _pcd_without_erosion = cameras.get_pcd_i(obj_masked_rgbds[j], cam_count) + # unmerged_obj_pcds.append(_pcd_without_erosion) + # view_pcd(_pcd) + # view_pcd(_pcd_without_erosion) + + # unmerged_obj_pcds += [ + # cameras.get_pcd_i(obj_masked_rgbd,cam_count) + # for obj_masked_rgbd in obj_masked_rgbds + # ] + # # make it + if not len(unmerged_obj_pcds): + + return 4 + obj_pcds = merge_pcds(unmerged_obj_pcds) + # + + + if len(obj_pcds) == 0: + print( + f"Failed to find any objects with mask size > {cfg.min_mask_size}!" + ) + return 1 + + # print(f"Finding ARTag") + # id_to_pcd = {} + # obj_centers = np.array([x.get_center() for x in obj_pcds]) + # for id, pose in id_to_pose.items(): + # dists = np.linalg.norm(obj_centers - pose, axis=1) + # argmin = dists.argmin() + # min = dists[argmin] + # if min < 0.05: + # id_to_pcd[id] = obj_pcds[argmin] + # print(f"Found object pointclouds corresponding to ARTag ids {list(id_to_pcd.keys())}") + cur_data = get_object_detect_dict(obj_pcds) + ref_data = read_object_detect_dict(pathname=Path( + hydra.utils.get_original_cwd(), + 'data', + 'category_to_rgb_map.json' + ).as_posix()) + + category_to_pcd_map = get_category_to_pcd_map(obj_pcds, cur_data, ref_data) + print(category_to_pcd_map) + + for _cat in category_order: + if _cat not in category_to_pcd_map.keys(): + continue + else: + if len(category_to_pcd_map[_cat]): + break + else: + continue + # + + # _cat = category_order[idx] + pcd_list = category_to_pcd_map.get(_cat, None) + if pcd_list is None: + # break #point() + return 2 + elif not len(pcd_list): + return 3 + _pcd = pcd_list[0] + # _pcd = corrected_pcd(np.asarray(_pcd.points)) + print('Selected category to grasp: ', _cat) + view_pcd(_pcd) + _pcd_extents = _pcd.get_max_bound() - _pcd.get_min_bound() + print(f'pcd outlier stats \n\n {_cat}: {_pcd_extents.tolist()} \n\n') + print('Set is_pcd_good=False to not execute grasp') + is_pcd_good = True + # + if is_pcd_good: + # replace obj_pcds with id_to_pcd[id] for grasping selected id + # for _cat, _pcd in category_to_pcd_map.items(): + print(f'Grasping {_cat}') + + # print("Getting grasps per object...") + obj_i, filtered_grasp_group = get_obj_grasps( + grasp_client, [_pcd], scene_pcd + ) + if obj_i is None: + print("Repeating the loop as couldn't find any suitable grasps") + continue + if len(filtered_grasp_group) <= 1: + print('No grasps from graspnet') + no_grasps_from_graspnet_counter += 1 + if no_grasps_from_graspnet_counter < threshold_no_grasps_from_graspnet_error : + continue + else: + print('Reposition and restart!!!') + exit(1) + # + # print("Choosing a grasp for the object") + chosen_grasp, final_filtered_grasps = robot.select_grasp( + filtered_grasp_group + ) + if chosen_grasp is None: + # print("Repeating the loop as CHOSEN grasp is None") + # continue + print('None of grasps were IK feasible. Selecting chosen grasp randomly from any of the infeasible IK grasps') + chosen_grasp = filtered_grasp_group[0] + # # grasp_client.visualize_grasp(scene_pcd, final_filtered_grasps) + # if len(final_filtered_grasps) <= 1: + # breakpoint() + # chosen_grasp = filtered_grasp_group[0] + else: + grasp_client.visualize_grasp( + _pcd, final_filtered_grasps, name=_cat + ) + k = 0 + success = False + # while not success and k < len(final_filtered_grasps): + # chosen_grasp = final_filtered_grasps[k] + success = execute_grasp(robot, chosen_grasp, time_to_go, place_in_drawer=True) + + print("Going home") + robot.go_home() + else: + continue + + +def corrected_pcd(points): + min_extent = np.array([0.17, -0.7, 0.]) + max_extent = np.array([0.7, -0.2, 0.4]) + clip_pcd_points = np.clip(points, min_extent, max_extent) + return o3d.geometry.PointCloud(o3d.utility.Vector3dVector(clip_pcd_points)) + +@hydra.main(config_path="../conf", config_name="run_grasp") +def main(cfg): + print(f"Config:\n{omegaconf.OmegaConf.to_yaml(cfg, resolve=True)}") + print(f"Current working directory: {os.getcwd()}") + + print("Initialize robot & gripper") + robot = hydra.utils.instantiate(cfg.robot) + robot.gripper_open() + robot.go_home() + + print("Initializing cameras") + cfg.cam.intrinsics_file = hydra.utils.to_absolute_path(cfg.cam.intrinsics_file) + cfg.cam.extrinsics_file = hydra.utils.to_absolute_path(cfg.cam.extrinsics_file) + cameras = hydra.utils.instantiate(cfg.cam) + breakpoint() + # print("Loading camera workspace masks") + # # import pdb; pdb.set_trace() + masks_1 = np.array( + [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_1], + dtype=np.float64, + ) + # masks_1[-1,:,:] *=0 + masks_2 = np.array( + [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_2], + dtype=np.float64, + ) + # masks_1[:2,:,:] *=0 + + print("Connect to grasp candidate selection and pointcloud processor") + segmentation_client = SegmentationClient() + grasp_client = GraspClient( + view_json_path=hydra.utils.to_absolute_path(cfg.view_json_path) + ) + + print("Loading ARTag modules") + frt_cams = [fairotag.CameraModule() for _ in range(cameras.n_cams)] + for frt, intrinsics in zip(frt_cams, cameras.intrinsics): + frt.set_intrinsics(intrinsics) + # MARKER_LENGTH = 0.04 + # MARKER_ID = [18] + # for i in MARKER_ID: + # for frt in frt_cams: + # frt.register_marker_size(i, MARKER_LENGTH) + + bottom_category_order = [ + 'dark_blue_plate', + 'pink_plate', + 'pink_small_bowl', + 'red_bowl', + 'purple_plate', + ] + top_category_order = [ + 'yellow_cup', + 'neon_yellow_plate', + 'light_blue_bowl', + 'aqua_plate', + ] + # pickplace_partial = partial(pickplace, cfg, + # masks_1, + # masks_2, + # root_working_dir, + # cameras, + # frt_cams, + # segmentation_client, + # grasp_client) + + root_working_dir = os.getcwd() + # # place all in sink + # done = False + # while not done: + # done = pickplace( + # robot, + # top_category_order + bottom_category_order, + # cfg, + # masks_1, + # masks_2, + # root_working_dir, + # cameras, + # frt_cams, + # segmentation_client, + # grasp_client + # # ) + open_top_drawer(robot) + robot.go_home() + done = 0 + while not done: + done = pickplace( + robot, + top_category_order, + cfg, + masks_1, + masks_2, + root_working_dir, + cameras, + frt_cams, + segmentation_client, + grasp_client + ) + close_top_drawer(robot) + open_bottom_drawer(robot) + robot.go_home() + done = 0 + while not done: + done = pickplace( + robot, + bottom_category_order, + cfg, + masks_1, + masks_2, + root_working_dir, + cameras, + frt_cams, + segmentation_client, + grasp_client + ) + close_bottom_drawer(robot) + robot.go_home() + + + +if __name__ == "__main__": + main() diff --git a/perception/sandbox/polygrasp/scripts/run_ttp.py b/perception/sandbox/polygrasp/scripts/run_ttp.py new file mode 100644 index 0000000000..4c241eeb7f --- /dev/null +++ b/perception/sandbox/polygrasp/scripts/run_ttp.py @@ -0,0 +1,786 @@ +#!/usr/bin/env python +""" +Connects to robot, camera(s), gripper, and grasp server. +Runs grasps generated from grasp server. +""" + + +#old +# high_position_open= torch.tensor([-2.8628, 1.7270, 1.8333, -1.6115, -1.6032, 1.4454, 0.4743]) +# top closed1 :tensor([-2.6614, 1.7002, 1.3380, -1.7611, -1.6574, 1.8371, 0.7491]) +# top closed2 : tensor([-2.4981, 1.7320, 1.0551, -1.6264, -1.6605, 2.1295, 0.7341]) +# bottom closed1: tensor([-2.5299, 1.6819, 0.8728, -1.4600, -1.5670, 2.3802, -2.7360]) +# bottom closed2: tensor([-2.3284, 1.7304, 0.6423, -1.2444, -1.4370, 2.5559, -2.7209]) +# top open: tensor([-2.7492, 1.7202, 1.0310, -1.1538, -1.3008, 2.1873, -2.8715]) +# bottom open: tensor([-2.4589, 1.7582, 0.5844, -0.9734, -1.1897, 2.4049, -2.8648]) + + +drawer_camera_index = 1 + +import time +import os +from unicodedata import category + +import numpy as np +import sklearn +import matplotlib.pyplot as plt + +import torch +import hydra +import omegaconf + +import json +import open3d as o3d +from pathlib import Path +from scipy.stats import mode +from functools import partial + +import polygrasp +from polygrasp.segmentation_rpc import SegmentationClient +from polygrasp.grasp_rpc import GraspClient +from polygrasp.serdes import load_bw_img + +import fairotag +from temporal_task_planner.utils.data_structure_utils import construct_rigid_instance, construct_act_instance +from temporal_task_planner.constants.gen_sess_config.lookup import dishwasher_part_poses, link_id_names, bounding_box_dict, category_vocab, special_instance_vocab +from temporal_task_planner.data_structures.state import State # Instance, RigidInstance +from temporal_task_planner.transform_hardware_to_sim import * # process_hw_pose_for_sim_axes +from temporal_task_planner.utils.datasetpytorch_utils import get_temporal_context +from temporal_task_planner.policy.learned_policy import PromptSituationLearnedPolicy as PromptSituationPolicy +from temporal_task_planner.trainer.transformer.dual_model import TransformerTaskPlannerDualModel +from temporal_task_planner.trainer.transformer.configs import TransformerTaskPlannerConfig as PromptSituationConfig +# from temporal_task_planner.trainer.submodule.category_encoder import CategoryEncoderMLP as CategoryEncoder +# from temporal_task_planner.trainer.submodule.pose_encoder import PoseEncoderFourierMLP as PoseEncoder +# from temporal_task_planner.trainer.submodule.temporal_encoder import TemporalEncoderEmbedding as TemporalEncoder +time_to_go = 5 + +def load_json(pathname): + with open(pathname, 'r') as f: + data = json.load(f) + return data + +map_cat_hw_to_sim = { + 'dark_blue_plate': "frl_apartment_plate_01_small_", + 'yellow_cup': "frl_apartment_kitchen_utensil_06_", + 'light_blue_bowl' : "frl_apartment_bowl_03_", + # 'peach_big_bowl': "frl_apartment_bowl_03_", + # 'red_bowl': "frl_apartment_bowl_07_small_", + 'pink_small_bowl': "frl_apartment_bowl_07_small_", + "bottom": "ktc_dishwasher_:0000_joint_1", + "top": "ktc_dishwasher_:0000_joint_3", +} +map_cat_sim_to_hw = {v: k for k, v in map_cat_hw_to_sim.items()} +X = np.load(os.path.expanduser('~') + '/temporal_task_planner/hw_transforms/pick_counter.npy') + +artag_lib = {} +artag_lib.update(load_json('data/all_closed.json')) +artag_lib.update(load_json('data/all_open.json')) +artag_lib.update(load_json('data/open_top_drawer.json')) +artag_lib.update(load_json('data/open_bottom_drawer.json')) + +top_closed_1 = torch.tensor([-2.8709, 1.7132, 1.3774, -1.8681, -1.4531, 1.9806, 0.4803]) +top_closed_2 = torch.tensor([-2.5949, 1.7388, 1.0075, -1.6537, -1.4691, 2.3417, 0.4605]) +top_open = torch.tensor([-2.8362, 1.7326, 1.0338, -1.2461, -1.4473, 2.2300, -0.0111]) +bottom_closed_1 = torch.tensor([-2.7429, 1.7291, 1.0249, -1.3325, -1.0166, 2.1604, -0.1720]) +bottom_closed_2 = torch.tensor([-2.1674, 1.6435, 0.5143, -1.2161, -0.9969, 2.5138, 0.2471]) +#torch.tensor([-2.2719, 1.675, 0.6623, -1.1511, -0.6550, 2.4067, -0.1707]) +bottom_open = torch.tensor([-2.4781, 1.7273, 0.5201, -0.9822, -0.9824, 2.4964, -0.0416]) +# torch.tensor([-2.2077, 1.65, 0.2767, -0.7902, -0.6421, 2.5545, -0.2362]) +high_position_close = torch.tensor([-2.8740, 1.3173, 1.5164, -1.2091, -1.1478, 1.4974, -0.1642]) +sink_pose = torch.tensor([-0.2135, -0.0278, 0.5381, -2.1573, 0.0384, 2.1235, -0.6401]) +# torch.tensor([-1.1165, 0.7988, 1.5438, -2.3060, -1.0097, 2.0797, -0.5347]) + +act_instance = construct_act_instance(is_action_available=False, is_action_to_be_predicted=True, relative_timestep=1) + +def move_to_joint_pos(robot, pos, time_to_go=5.0): + state_log = [] + while len(state_log) < time_to_go*100: + state_log = robot.move_to_joint_positions(pos, time_to_go) + return state_log + + +def open_bottom_drawer(robot): + traj = robot.move_to_joint_positions(high_position_close) + traj = robot.move_to_joint_positions(bottom_closed_1) + traj = robot.move_to_joint_positions(bottom_closed_2) + traj = robot.move_to_joint_positions(bottom_open) + traj = robot.move_to_joint_positions(high_position_close) + +def open_top_drawer(robot): + traj = robot.move_to_joint_positions(high_position_close) + traj = robot.move_to_joint_positions(top_closed_1) + traj = robot.move_to_joint_positions(top_closed_2) + traj = robot.move_to_joint_positions(top_open) + traj = robot.move_to_joint_positions(high_position_close) + +def close_top_drawer(robot): + traj = robot.move_to_joint_positions(high_position_close) + traj = robot.move_to_joint_positions(top_open) + traj = robot.move_to_joint_positions(top_closed_2) + traj = robot.move_to_joint_positions(top_closed_1) + traj = robot.move_to_joint_positions(high_position_close) + +def close_bottom_drawer(robot): + traj = robot.move_to_joint_positions(high_position_close) + traj = robot.move_to_joint_positions(bottom_open) + traj = robot.move_to_joint_positions(bottom_closed_2) + traj = robot.move_to_joint_positions(bottom_closed_1) + traj = robot.move_to_joint_positions(high_position_close) + +def view_pcd(pcd): + o3d.visualization.draw_geometries([pcd]) + return + +def save_depth_images(rgbd): + depth_img = rgbd[:,:,-1] + + + +def get_object_detect_dict(obj_pcds): + data = {} + for idx, pcd in enumerate(obj_pcds): + data[idx] = mode(pcd.colors, axis=0).mode.tolist() + return data + + +# only to record the rgb mode values and manually label +def save_object_detect_dict(obj_pcds): + data = get_object_detect_dict(obj_pcds) + with open('category_to_rgb_mapping.json', 'w') as f: + json.dump(data, f, indent=4) + return + + +def is_shadow(val, epsilon=5e-3): + err_01 = abs(val[0] - val[1]) + err_21 = abs(val[2] - val[1]) + err_02 = abs(val[2] - val[0]) + if err_01 < epsilon and err_21 < epsilon and err_02 < epsilon: + return True + return False + +def is_not_on_counter(xyz): + if xyz[-1] > 0: + return False + return True + +def get_category_to_pcd_map(obj_pcds, cur_data, ref_data): + category_to_pcd_map = {cat: [] for cat in ref_data} + for idx, val in cur_data.items(): + min_err = 10000 + min_err_category = 'ignore' # default + if is_shadow(val[0]) or is_not_on_counter(obj_pcds[idx].get_center()): + continue + for category, rgb_value in ref_data.items(): + err = np.linalg.norm(np.array(val) - np.array(rgb_value)) + if min_err > err: + min_err = err + min_err_category = category + if min_err_category.startswith('ignore_'): + continue + category_to_pcd_map[min_err_category].append(obj_pcds[idx]) + return category_to_pcd_map + + +def save_rgbd(rgbd): + num_cams = rgbd.shape[0] + f, axarr = plt.subplots(2, num_cams) + + for i in range(num_cams): + if num_cams > 1: + ax1, ax2 = axarr[0, i], axarr[1, i] + else: + ax1, ax2 = axarr + ax1.imshow(rgbd[i, :, :, :3].astype(np.uint8)) + ax2.matshow(rgbd[i, :, :, 3:]) + + f.savefig("rgbd.png") + plt.close(f) + +def save_rgbd_masked(rgbd, rgbd_masked): + num_cams = rgbd.shape[0] + f, axarr = plt.subplots(2, num_cams) + + for i in range(num_cams): + if num_cams > 1: + ax1, ax2 = axarr[0, i], axarr[1, i] + else: + ax1, ax2 = axarr + ax1.imshow(rgbd[i, :, :, :3].astype(np.uint8)) + ax2.imshow(rgbd_masked[i, :, :, :3].astype(np.uint8)) + + f.savefig("rgbd_masked.png") + plt.close(f) + + +def get_obj_grasps(grasp_client, obj_pcds, scene_pcd): + for obj_i, obj_pcd in enumerate(obj_pcds): + print(f"Getting obj {obj_i} grasp...") + grasp_group = grasp_client.get_grasps(obj_pcd) + filtered_grasp_group = grasp_client.get_collision(grasp_group, scene_pcd) + if len(filtered_grasp_group) < len(grasp_group): + print( + "Filtered" + f" {len(grasp_group) - len(filtered_grasp_group)}/{len(grasp_group)} grasps" + " due to collision." + ) + if len(filtered_grasp_group) > 0: + return obj_i, filtered_grasp_group + raise Exception( + "Unable to find any grasps after filtering, for any of the" + f" {len(obj_pcds)} objects" + ) + + +def merge_pcds(pcds, eps=0.1, min_samples=2): + """Cluster object pointclouds from different cameras based on centroid using DBSCAN; merge when within eps""" + xys = np.array([pcd.get_center()[:2] for pcd in pcds]) + cluster_labels = ( + sklearn.cluster.DBSCAN(eps=eps, min_samples=min_samples).fit(xys).labels_ + ) + + # Logging + total_n_objs = len(xys) + total_clusters = cluster_labels.max() + 1 + unclustered_objs = (cluster_labels < 0).sum() + print( + f"Clustering objects from all cameras: {total_clusters} clusters, plus" + f" {unclustered_objs} non-clustered objects; went from {total_n_objs} to" + f" {total_clusters + unclustered_objs} objects" + ) + + # Cluster label == -1 when unclustered, otherwise cluster label >=0 + final_pcds = [] + cluster_to_pcd = dict() + for cluster_label, pcd in zip(cluster_labels, pcds): + if cluster_label >= 0: + if cluster_label not in cluster_to_pcd: + cluster_to_pcd[cluster_label] = pcd + else: + cluster_to_pcd[cluster_label] += pcd + else: + final_pcds.append(pcd) + + return list(cluster_to_pcd.values()) + final_pcds + + +def execute_grasp(robot, chosen_grasp, time_to_go, place_in_drawer=False): + traj, success = robot.grasp( + chosen_grasp, time_to_go=time_to_go, gripper_width_success_threshold=0.001 + ) + print(f"Grasp success: {success}") + breakpoint() + if success: + ## place in sink + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0, 0.3]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + curr_pose, curr_ori = robot.get_ee_pose() + + if place_in_drawer: + print("Placing object in hand in drawer...") + print("Moving horizontally") + traj += robot.move_until_success( + position=torch.Tensor([-0.09, -0.61, 0.2]), + orientation=[1,0,0,0], + time_to_go=time_to_go, + ) + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving down") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0.0, -0.20]), + orientation=[1,0,0,0], + time_to_go=time_to_go, + ) + else: + print("Moving to sink pose") + traj += move_to_joint_pos(robot, sink_pose) + + print("Opening gripper") + robot.gripper_open() + curr_pose, curr_ori = robot.get_ee_pose() + print("Moving up") + traj += robot.move_until_success( + position=curr_pose + torch.Tensor([0, 0.0, 0.3]), + orientation=curr_ori, + time_to_go=time_to_go, + ) + robot.go_home() + return traj + +def pickplace( + robot, + category_order, + cfg, + masks_1, + masks_2, + root_working_dir, + cameras, + frt_cams, + segmentation_client, + grasp_client, +): + for outer_i in range(cfg.num_bin_shifts): + cam_i = outer_i % 2 + print(f"=== Starting bin shift with cam {cam_i} ===") + + # # # Define some parameters for each workspace. + # if cam_i == 0: + # masks = masks_1 + # hori_offset = torch.Tensor([0, -0.4, 0]) + # else: + # masks = masks_2 + # hori_offset = torch.Tensor([0, 0.4, 0]) + time_to_go = 5 + + for i in range(cfg.num_grasps_per_bin_shift): + # Create directory for current grasp iteration + os.chdir(root_working_dir) + timestamp = int(time.time()) + os.makedirs(f"{timestamp}") + os.chdir(f"{timestamp}") + + print( + f"=== Grasp {i + 1}/{cfg.num_grasps_per_bin_shift}, logging to" + f" {os.getcwd()} ===" + ) + + print("Getting rgbd and pcds..") + rgbd = cameras.get_rgbd() + rgbd_masked = rgbd + + print("Detecting markers & their corresponding points") + uint_rgbs = rgbd[:,:,:,:3].astype(np.uint8) + id_to_pose = {} + for i, (frt, uint_rgb) in enumerate(zip(frt_cams, uint_rgbs)): + markers = frt.detect_markers(uint_rgb) + for marker in markers: + w_i, h_i = marker.corner.mean(axis=0).round().astype(int) + + single_point = np.zeros_like(rgbd[i]) + single_point[h_i, w_i, :] = rgbd[i, h_i, w_i, :] + pcd = cameras.get_pcd_i(single_point, i) + xyz = pcd.get_center() + id_to_pose[marker.id] = xyz + + scene_pcd = cameras.get_pcd(rgbd) + + + save_rgbd(rgbd) + + print("Segmenting image...") + unmerged_obj_pcds = [] + for i in range(cameras.n_cams): + obj_masked_rgbds, obj_masks = segmentation_client.segment_img( + rgbd_masked[i], min_mask_size=cfg.min_mask_size + ) + unmerged_obj_pcds += [ + cameras.get_pcd_i(obj_masked_rgbd, i) + for obj_masked_rgbd in obj_masked_rgbds + ] + + obj_pcds = merge_pcds(unmerged_obj_pcds) + # + + + if len(obj_pcds) == 0: + print( + f"Failed to find any objects with mask size > {cfg.min_mask_size}!" + ) + return 1 + + # print(f"Finding ARTag") + # id_to_pcd = {} + # obj_centers = np.array([x.get_center() for x in obj_pcds]) + # for id, pose in id_to_pose.items(): + # dists = np.linalg.norm(obj_centers - pose, axis=1) + # argmin = dists.argmin() + # min = dists[argmin] + # if min < 0.05: + # id_to_pcd[id] = obj_pcds[argmin] + # print(f"Found object pointclouds corresponding to ARTag ids {list(id_to_pcd.keys())}") + cur_data = get_object_detect_dict(obj_pcds) + ref_data = load_json(pathname=Path( + hydra.utils.get_original_cwd(), + 'data', + 'category_to_rgb_map.json' + ).as_posix()) + + # + category_to_pcd_map = get_category_to_pcd_map(obj_pcds, cur_data, ref_data) + + for _cat in category_order: + if _cat not in category_to_pcd_map.keys(): + continue + else: + break + # _cat = category_order[idx] + _pcd = category_to_pcd_map.get(_cat, None) + if _pcd is None: + # break #point() + return 2 + # replace obj_pcds with id_to_pcd[id] for grasping selected id + # for _cat, _pcd in category_to_pcd_map.items(): + print(f'Grasping {_cat}') + + # print("Getting grasps per object...") + obj_i, filtered_grasp_group = get_obj_grasps( + grasp_client, [_pcd], scene_pcd + ) + + # print("Choosing a grasp for the object") + chosen_grasp, final_filtered_grasps = robot.select_grasp( + filtered_grasp_group + ) + grasp_client.visualize_grasp(scene_pcd, final_filtered_grasps) + grasp_client.visualize_grasp( + _pcd, final_filtered_grasps, name=_cat + ) + + traj = execute_grasp(robot, chosen_grasp, time_to_go) + + print("Going home") + robot.go_home() + +def transform_hw_to_sim_pose(pcd): + """converts the 3d coordinates in hw robot frame to + sim's default world frame + 1. load the transform + 2. convert hw coordinates in correct format for multiply + """ + # xyz_hw_in_sim_axes + xyz_A = process_hw_pose_for_sim_axes(pcd.get_center()) + b = get_simulation_coordinates(xyz_A=xyz_A, X=X) + return b + [1.0,0.0,0.0,0.0] # default quaternion + +# def transform_hw_to_sim_category(category_to_pcd_map): + +# category_name=[map_cat_hw_to_sim[key] for key in category_to_pcd_map] +# category_bb = [map_cat_tok_to_bb[cat] for cat in category_name] +# return { +# 'category_name': category_name, +# 'category': category_bb +# } + +def get_hardware_countertop_objects(category_to_pcd_map): + rigid_instances = [] + for colorbasedname, pcd_list in category_to_pcd_map.items(): + for i, pcd in enumerate(pcd_list): + category_name = map_cat_hw_to_sim[colorbasedname] + name = "{}:{:04d}".format(category_name, i) + pose = transform_hw_to_sim_pose(pcd) + entry = construct_rigid_instance(name, pose, category_name, relative_timestep=1) + # RigidInstance( + # timestep=1, + # category=bounding_box_dict[category_name], + # pose=pose, + # action_masks=False, + # is_real=True, + # category_token_name=category_name, # record utensil type + # category_token=category_vocab.word2index(category_name), + # instance_token=special_instance_vocab.word2index(name), + # category_name=category_name, + # instance_name=name, + # ) + rigid_instances.append(entry) + return rigid_instances + + +def get_category_pcd_on_counter(cfg, cameras, segmentation_client): + rgbd = cameras.get_rgbd() + scene_pcd = cameras.get_pcd(rgbd) + print("Segmenting image...") + unmerged_obj_pcds = [] + for i in range(cameras.n_cams): + obj_masked_rgbds, obj_masks = segmentation_client.segment_img( + rgbd[i], min_mask_size=cfg.min_mask_size + ) + unmerged_obj_pcds += [ + cameras.get_pcd_i(obj_masked_rgbd, i) + for obj_masked_rgbd in obj_masked_rgbds + ] + obj_pcds = merge_pcds(unmerged_obj_pcds) + cur_data = get_object_detect_dict(obj_pcds) + ref_data = load_json(pathname=Path( + hydra.utils.get_original_cwd(), + 'data', + 'category_to_rgb_map.json' + ).as_posix()) + # + category_to_pcd_map = get_category_to_pcd_map(obj_pcds, cur_data, ref_data) + # rigid_instances = get_hardware_rigid_instance(category_to_pcd_map) + # # # instance_name_list = # count instance name + # # category_dict = transform_hw_to_sim_category(category_to_pcd_map) + # # pose_list = transform_hw_to_sim_pose(category_to_pcd_map) + # # timestep_list = [1.]*len(pose_list) + # # is_real_list = [1.]*len(pose_list) + # # State() + # + + # situation = { + # 'timestep': torch.tensor([timestep_list]) + # } + # # transform_position_counter_hw2sim = np.load(os.path.expanduser('~') + '/temporal_task_planner/hw_transforms/pick_counter.npy') + # # transform_category_hw2sim = + # # # return category_to_pcd_map + return category_to_pcd_map + +def get_marker_corners(cameras, frt_cams): + rgbd = cameras.get_rgbd() + rgbd_masked = rgbd + save_rgbd(rgbd) + uint_rgbs = rgbd[:,:,:,:3].astype(np.uint8) + drawer_markers = frt_cams[drawer_camera_index].detect_markers(uint_rgbs[drawer_camera_index]) + # print(drawer_markers) + data = [{ + "id": int(m.id), + "corner": m.corner.astype(np.int32).tolist() + } for m in drawer_markers] + return data + +def get_drawer_as_dishwasher_instances(cameras, frt_cams, drawer_status): + # measure similarity to the 4 scenarios of artags + # and set the drawer poses in sim + data = get_marker_corners(cameras, frt_cams) + # # check the position of marker tags corners and take the most similar + # most_likely_status = 'all_closed' + # min_err = 10000000 + # for key, markers in artag_lib.items(): + # for ref_marker_info in markers: + # for cur_marker_info in data: + # if cur_marker_info['id'] == ref_marker_info['id']: + # # compare the norm of the difference in corner matrices + # flag_match = True + # else: + # flag_match = False + # if flag_match: + # err = np.linalg.norm(cur_marker_info['corner'] - ref_marker_info['corner']) + # if min_err > err: + # min_err = err + # most_likely_status = key + door = construct_rigid_instance( + name=link_id_names["door"], + pose=dishwasher_part_poses['door']['open'], + category_name=link_id_names["door"], + relative_timestep=1 + ) + + # if status == 'all_closed': + # default + bottom_rack = construct_rigid_instance( + name=link_id_names['bottom'], + pose=dishwasher_part_poses['bottom']['close'], + category_name=link_id_names['bottom'], + relative_timestep=1 + ) + top_rack = construct_rigid_instance( + name=link_id_names["top"], + pose=dishwasher_part_poses['top']['close'], + category_name=link_id_names["top"], + relative_timestep=1 + ) + if drawer_status['top'] == 'open': + top_rack = construct_rigid_instance( + name=link_id_names["top"], + pose=dishwasher_part_poses['top']['open'], + category_name=link_id_names["top"], + relative_timestep=1 + ) + elif drawer_status['bottom'] == 'open': + bottom_rack = construct_rigid_instance( + name=link_id_names['bottom'], + pose=dishwasher_part_poses['bottom']['open'], + category_name=link_id_names['bottom'], + relative_timestep=1 + ) + return [door, bottom_rack, top_rack] + +@hydra.main(config_path="../conf", config_name="run_ttp") +def main(cfg): + drawer_status = { + 'top': 'close', + 'bottom': 'close' + } + print('drawer_status: ', drawer_status) + print('Do you want to modify drawer_status ???') + breakpoint() + # config = hydra.utils.instantiate(cfg.config) + config = PromptSituationConfig( + num_instances=60, + d_model=cfg.d_model, + nhead=2, + d_hid=cfg.d_hid, + num_slots=cfg.num_slots, + slot_iters=cfg.slot_iters, + num_encoder_layers=cfg.num_encoder_layers, + num_decoder_layers=cfg.num_decoder_layers, + dropout=0., + batch_first=True, + category_embed_size=cfg.category_embed_size, + pose_embed_size=cfg.pose_embed_size, + temporal_embed_size=cfg.temporal_embed_size, + marker_embed_size=cfg.marker_embed_size, + ) + category_encoder = hydra.utils.instantiate(cfg.category_encoder) + temporal_encoder = hydra.utils.instantiate(cfg.temporal_encoder) + pose_encoder = hydra.utils.instantiate(cfg.pose_encoder) + reality_marker_encoder = hydra.utils.instantiate(cfg.reality_marker_encoder) + model = TransformerTaskPlannerDualModel( + config, + category_encoder=category_encoder, + temporal_encoder=temporal_encoder, + pose_encoder=pose_encoder, + reality_marker_encoder=reality_marker_encoder, + ) + model.load_state_dict(torch.load(cfg.checkpoint_path)['model_state_dict']) #, map_location='cpu') + policy = PromptSituationPolicy(model, pick_only=False, device='cpu') + + prompt_temporal_context = get_temporal_context(cfg.prompt_session_path) + prompt_temporal_context.pick_only = True + prompt = prompt_temporal_context.process_states() + for key, val in prompt.items(): + prompt[key] = torch.tensor(val).unsqueeze(0) + prompt_input_len = len(prompt["timestep"][0]) + prompt["src_key_padding_mask"] = ( + torch.zeros(prompt_input_len).bool().unsqueeze(0) + ) + policy.reset(prompt) + + print(f"Config:\n{omegaconf.OmegaConf.to_yaml(cfg, resolve=True)}") + print(f"Current working directory: {os.getcwd()}") + + print("Initialize robot & gripper") + robot = hydra.utils.instantiate(cfg.robot) + robot.gripper_open() + robot.go_home() + + print("Initializing cameras") + cfg.cam.intrinsics_file = hydra.utils.to_absolute_path(cfg.cam.intrinsics_file) + cfg.cam.extrinsics_file = hydra.utils.to_absolute_path(cfg.cam.extrinsics_file) + cameras = hydra.utils.instantiate(cfg.cam) + + + print("Loading camera workspace masks") + # # import pdb; pdb.set_trace() + masks_1 = np.array( + [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_1], + dtype=np.float64, + ) + masks_1[1,:,:] *=0 + masks_2 = np.array( + [load_bw_img(hydra.utils.to_absolute_path(x)) for x in cfg.masks_2], + dtype=np.float64, + ) + # masks_1[:2,:,:] *=0 + masks_1[0,:,:] *=0 + masks_1[2,:,:] *=0 + + # if cam_i == 0: + # masks = masks_1 + # hori_offset = torch.Tensor([0, -0.4, 0]) + # else: + # masks = masks_2 + # hori_offset = torch.Tensor([0, 0.4, 0]) + hori_offset = torch.Tensor([0.,0.,0.]) + + print("Connect to grasp candidate selection and pointcloud processor") + segmentation_client = SegmentationClient() + grasp_client = GraspClient( + view_json_path=hydra.utils.to_absolute_path(cfg.view_json_path) + ) + + print("Loading ARTag modules") + frt_cams = [fairotag.CameraModule() for _ in range(cameras.n_cams)] + for frt, intrinsics in zip(frt_cams, cameras.intrinsics): + frt.set_intrinsics(intrinsics) + MARKER_LENGTH = 0.04 + MARKER_ID = [22, 27, 26, 23, 29] #[18] + for i in MARKER_ID: + for frt in frt_cams: + frt.register_marker_size(i, MARKER_LENGTH) + step = -1 + + while True: + step += 1 + rgbd = cameras.get_rgbd() + scene_pcd = cameras.get_pcd(rgbd) + + category_to_pcd_map = get_category_pcd_on_counter(cfg, cameras, segmentation_client) + # save categories seen with their point clouds. + objdetect_on_counter = [] + for cat, pcd_list in category_to_pcd_map.items(): + if len(pcd_list): + objdetect_on_counter.append(cat) + for i, pcd in enumerate(pcd_list): + o3d.io.write_point_cloud(f'step-{step}_{cat}-{i}.pcd', pcd) + + dw_parts = get_drawer_as_dishwasher_instances(cameras, frt_cams, drawer_status) + dishes = get_hardware_countertop_objects(category_to_pcd_map) + rigid_instances = dw_parts + dishes + + current_state = State(rigid_instances=rigid_instances, act_instances=act_instance) + print(current_state) + action = policy.get_action(current_state) + breakpoint() + print(map_cat_sim_to_hw.get(action.pick_instance.category_name, None)) + if action.pick_instance.category_name == link_id_names["door"]: # "ktc_dishwasher_:0000_joint_2": + break + elif action.pick_instance.category_name == link_id_names["bottom"]: # "ktc_dishwasher_:0000_joint_1": + if abs(action.get_place_position()[2] - 0.95) < 1e-3: + if drawer_status['top'] == 'open': + break + open_bottom_drawer(robot) + drawer_status['bottom'] = "open" + else: + if drawer_status['top'] == 'open': + break + close_bottom_drawer(robot) + drawer_status['bottom'] = "close" + elif action.pick_instance.category_name == link_id_names["top"]: # "ktc_dishwasher_:0000_joint_3": + if abs(action.get_place_position()[2] - 0.95) < 1e-3: + open_top_drawer(robot) + drawer_status['top'] = "open" + else: + close_top_drawer(robot) + drawer_status['top'] = "close" + elif action.pick_instance.category_name in map_cat_sim_to_hw: + _cat = map_cat_sim_to_hw[ action.pick_instance.category_name] + _pcd = category_to_pcd_map[_cat][0] + if _pcd is None: + # break #point() + return 2 + # replace obj_pcds with id_to_pcd[id] for grasping selected id + # for _cat, _pcd in category_to_pcd_map.items(): + # print(f'Grasping {_cat}') + + # print("Getting grasps per object...") + obj_i, filtered_grasp_group = get_obj_grasps( + grasp_client, [_pcd], scene_pcd + ) + + # print("Choosing a grasp for the object") + chosen_grasp, final_filtered_grasps = robot.select_grasp( + filtered_grasp_group + ) + grasp_client.visualize_grasp(scene_pcd, final_filtered_grasps) + grasp_client.visualize_grasp( + _pcd, final_filtered_grasps, name=_cat + ) + if abs(action.get_place_position()[1] - 0.9599) < 1e-3: + place_in_drawer = False + else: + place_in_drawer = True + traj = execute_grasp(robot, chosen_grasp, time_to_go, place_in_drawer) + + print("Going home") + robot.go_home() + + + + +if __name__ == "__main__": + main() diff --git a/perception/sandbox/polygrasp/setup.py b/perception/sandbox/polygrasp/setup.py new file mode 100644 index 0000000000..85efd6c87a --- /dev/null +++ b/perception/sandbox/polygrasp/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup, find_packages + +""" +Wrapper package defining the interface for grasping primitives in Fairo. +""" + +__author__ = "Yixin Lin" +__copyright__ = "2022, Meta" + + +install_requires = [ + "mrp", + "graspnetAPI", + "open3d", + "fairomsg", + "realsense_wrapper", + "ikpy", # TODO: switch to better IK lib +] + +setup( + name="polygrasp", + author="Yixin Lin", + author_email="yixinlin@fb.com", + version="0.1", + packages=find_packages(where="src"), + package_dir={"": "src"}, + include_package_data=True, + scripts=["scripts/run_grasp.py"], + install_requires=install_requires, +) diff --git a/perception/sandbox/polygrasp/src/polygrasp/__init__.py b/perception/sandbox/polygrasp/src/polygrasp/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/perception/sandbox/polygrasp/src/polygrasp/cam_pub_sub.py b/perception/sandbox/polygrasp/src/polygrasp/cam_pub_sub.py new file mode 100644 index 0000000000..2910082cc5 --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp/cam_pub_sub.py @@ -0,0 +1,136 @@ +import logging +import json +from types import SimpleNamespace + +import numpy as np +import open3d as o3d + +import a0 +from polygrasp import serdes + +log = logging.getLogger(__name__) +topic = "cams/rgbd" + + +class CameraSubscriber: + def __init__(self, intrinsics_file, extrinsics_file): + with open(intrinsics_file, "r") as f: + intrinsics_json = json.load(f) + self.intrinsics = [SimpleNamespace(**d) for d in intrinsics_json] + + with open(extrinsics_file, "r") as f: + self.extrinsics = json.load(f) + + assert len(self.intrinsics) == len(self.extrinsics) + self.n_cams = len(self.intrinsics) + + self.sub = a0.SubscriberSync(topic, a0.INIT_MOST_RECENT, a0.ITER_NEWEST) + self.recent_rgbd = None + + def get_rgbd(self): + if self.sub.can_read(): + self.recent_rgbd = serdes.bytes_to_np(self.sub.read().payload) + return self.recent_rgbd + + +class PointCloudSubscriber(CameraSubscriber): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + self.width = self.intrinsics[0].width + self.height = self.intrinsics[0].height + + # Convert to open3d intrinsics + self.o3_intrinsics = [ + o3d.camera.PinholeCameraIntrinsic( + width=intrinsic.width, + height=intrinsic.height, + fx=intrinsic.fx, + fy=intrinsic.fy, + cx=intrinsic.ppx, + cy=intrinsic.ppy, + ) + for intrinsic in self.intrinsics + ] + + # Convert to numpy homogeneous transforms + self.extrinsic_transforms = np.empty([self.n_cams, 4, 4]) + for i, calibration in enumerate(self.extrinsics): + self.extrinsic_transforms[i] = np.eye(4) + self.extrinsic_transforms[i, :3, :3] = calibration["camera_base_ori"] + self.extrinsic_transforms[i, :3, 3] = calibration["camera_base_pos"] + + def get_pcd_i(self, rgbd: np.ndarray, cam_i: int, mask: np.ndarray = None): + if mask is None: + mask = np.ones([self.height, self.width]) + + intrinsic = self.o3_intrinsics[cam_i] + transform = self.extrinsic_transforms[cam_i] + + # The specific casting here seems to be very important, even though + # rgbd should already be in np.uint16 type... + img = (rgbd[:, :, :3] * mask[:, :, None]).astype(np.uint8) + depth = (rgbd[:, :, 3] * mask).astype(np.uint16) + + o3d_img = o3d.cuda.pybind.geometry.Image(img) + o3d_depth = o3d.cuda.pybind.geometry.Image(depth) + + rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth( + o3d_img, + o3d_depth, + convert_rgb_to_intensity=False, + ) + + pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic) + pcd.transform(transform) + + return pcd + + def get_pcd( + self, rgbds: np.ndarray, masks: np.ndarray = None + ) -> o3d.geometry.PointCloud: + if masks is None: + masks = np.ones([self.n_cams, self.height, self.width]) + pcds = [self.get_pcd_i(rgbds[i], i, masks[i]) for i in range(len(rgbds))] + result = pcds[0] + for pcd in pcds[1:]: + result += pcd + return result + + +if __name__ == "__main__": + import argparse + import realsense_wrapper + + parser = argparse.ArgumentParser() + parser.add_argument( + "--intrinsics", + type=str, + default="conf/intrinsics.json", + help="JSON file to overwrite with current intrinsics.", + ) + args = parser.parse_args() + cameras = realsense_wrapper.RealsenseAPI() + + intrinsics = cameras.get_intrinsics() + intrinsics_py = [ + dict( + coeffs=x.coeffs, + fx=x.fx, + fy=x.fy, + height=x.height, + ppx=x.ppx, + ppy=x.ppy, + width=x.width, + ) + for x in intrinsics + ] + with open(args.intrinsics, "w") as f: + json.dump(intrinsics_py, f, indent=4) + + rgbd_pub = a0.Publisher(topic) + + log.info(f"Starting camera logger with {cameras.get_num_cameras()} cameras...") + while True: + img_bytes = serdes.np_to_bytes(cameras.get_rgbd()) + rgbd_pub.pub(img_bytes) diff --git a/perception/sandbox/polygrasp/src/polygrasp/grasp_rpc.py b/perception/sandbox/polygrasp/src/polygrasp/grasp_rpc.py new file mode 100644 index 0000000000..f8075bd09c --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp/grasp_rpc.py @@ -0,0 +1,170 @@ +import logging +import numpy as np +import open3d as o3d +from matplotlib import pyplot as plt + +import a0 + +import graspnetAPI +from polygrasp import serdes +from polygrasp.serdes import polygrasp_msgs + +import signal + + +log = logging.getLogger(__name__) +topic_key = "grasp_server" +grasp_topic_key = f"{topic_key}/grasp" +collision_topic_key = f"{topic_key}/collision" + + +def save_img(img, name): + f = plt.figure() + plt.imshow(img) + f.savefig(f"{name}.png") + plt.close(f) + + +class GraspServer: + def _get_grasps(self, pcd: o3d.geometry.PointCloud) -> np.ndarray: + raise NotImplementedError + + def _get_collisions( + self, grasps: graspnetAPI.GraspGroup, scene_pcd: o3d.geometry.PointCloud + ) -> graspnetAPI.GraspGroup: + raise NotImplementedError + + def start(self): + log.info(f"Starting grasp server...") + + def grasp_onrequest(req): + log.info(f"Got request; computing grasp group...") + + payload = req.pkt.payload + pcd = serdes.capnp_to_pcd(payload) + grasp_group = self._get_grasps(pcd) + + log.info(f"Done. Replying with serialized grasp group...") + req.reply(serdes.grasp_group_to_capnp(grasp_group).to_bytes()) + + def collision_onrequest(req): + log.info(f"Got request; computing collisions...") + + payload = req.pkt.payload + msg = polygrasp_msgs.CollisionRequest.from_bytes(payload) + grasp_group = serdes.bytes_to_grasp_group(msg.grasps) + scene_pcd = serdes.capnp_to_pcd(msg.pcd) + + filtered_grasp_group = self._get_collisions(grasp_group, scene_pcd) + log.info(f"Done. Replying with serialized filtered grasps...") + req.reply(serdes.grasp_group_to_bytes(filtered_grasp_group)) + + self.grasp_server = a0.RpcServer(grasp_topic_key, grasp_onrequest, None) + self.collision_server = a0.RpcServer( + collision_topic_key, collision_onrequest, None + ) + + signal.pause() + + +class GraspClient: + def __init__(self, view_json_path): + self.grasp_client = a0.RpcClient(grasp_topic_key) + self.collision_client = a0.RpcClient(collision_topic_key) + self.view_json_path = view_json_path + + def downsample_pcd( + self, pcd: o3d.geometry.PointCloud, max_num_bits=8 * 1024 * 1024 + ): + # a0 default max msg size 16MB; make sure every msg < 1/2 of max + i = 1 + while True: + downsampled_pcd = pcd.uniform_down_sample(i) + bits = serdes.pcd_to_capnp(downsampled_pcd).to_bytes() + if len(bits) > max_num_bits: + log.warning(f"Downsampling pointcloud...") + i += 1 + else: + break + if i > 1: + log.warning(f"Downsampled to every {i}th point.") + + return bits + + def get_grasps(self, pcd: o3d.geometry.PointCloud) -> graspnetAPI.GraspGroup: + bits = self.downsample_pcd(pcd) + result_bits = self.grasp_client.send_blocking(bits).payload + return serdes.capnp_to_grasp_group(result_bits) + + def get_collision( + self, grasps: graspnetAPI.GraspGroup, scene_pcd: o3d.geometry.PointCloud + ): + request = polygrasp_msgs.CollisionRequest() + request.pcd = self.downsample_pcd(scene_pcd) + request.grasps = serdes.grasp_group_to_bytes(grasps) + bits = request.to_bytes() + result_bits = self.collision_client.send_blocking(bits).payload + + return serdes.bytes_to_grasp_group(result_bits) + + def visualize(self, scene_pcd, plot=False, render=False, save_view=False): + vis = o3d.visualization.Visualizer() + vis.create_window() + vis.add_geometry(scene_pcd, reset_bounding_box=True) + + if render: + """Render the window. You can rotate it & save the view.""" + # Actually render the window: + log.info(f"Rendering scene in Open3D") + vis.run() + param = vis.get_view_control().convert_to_pinhole_camera_parameters() + if save_view: + log.info(f"Saving new view to {self.view_json_path}") + # Save the view + o3d.io.write_pinhole_camera_parameters(self.view_json_path, param) + + param = o3d.io.read_pinhole_camera_parameters(self.view_json_path) + vis.get_view_control().convert_from_pinhole_camera_parameters(param) + + return vis + + def visualize_grasp( + self, + scene_pcd: o3d.geometry.PointCloud, + grasp_group: graspnetAPI.GraspGroup, + n=5, + plot=False, + render=False, + save_view=False, + name="scene", + ) -> None: + grasp_o3d_geometries = grasp_group.to_open3d_geometry_list() + grasp_pointclouds = [ + grasp_o3d_geometry.sample_points_uniformly(number_of_points=5000) + for grasp_o3d_geometry in grasp_o3d_geometries + ] + vis = self.visualize( + scene_pcd=scene_pcd, plot=plot, render=render, save_view=save_view + ) + + # Save scene + grasp_image = np.array(vis.capture_screen_float_buffer(do_render=True)) + save_img(grasp_image, name) + + n = min(n, len(grasp_o3d_geometries)) + log.info(f"Visualizing top {n} grasps in Open3D...") + + # Save scene with each top grasp individually + for i, grasp_pointcloud in enumerate(grasp_pointclouds[:n]): + vis.add_geometry(grasp_pointcloud, reset_bounding_box=False) + grasp_image = np.array(vis.capture_screen_float_buffer(do_render=True)) + save_img(grasp_image, f"{name}_with_grasp_{i + 1}") + vis.remove_geometry(grasp_pointcloud, reset_bounding_box=False) + + # Save scene with all grasps + for grasp_pointcloud in grasp_pointclouds[:n]: + vis.add_geometry(grasp_pointcloud, reset_bounding_box=False) + grasp_image = np.array(vis.capture_screen_float_buffer(do_render=True)) + save_img(grasp_image, f"{name}_with_grasps") + + return vis diff --git a/perception/sandbox/polygrasp/src/polygrasp/polygrasp.capnp b/perception/sandbox/polygrasp/src/polygrasp/polygrasp.capnp new file mode 100644 index 0000000000..6172d073db --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp/polygrasp.capnp @@ -0,0 +1,6 @@ +@0xffe8a4b298f1916c; +using Cxx = import "/capnp/c++.capnp"; +struct CollisionRequest { + pcd @0 :Data; + grasps @1 :Data; +} \ No newline at end of file diff --git a/perception/sandbox/polygrasp/src/polygrasp/robot_interface.py b/perception/sandbox/polygrasp/src/polygrasp/robot_interface.py new file mode 100644 index 0000000000..d800cd5c30 --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp/robot_interface.py @@ -0,0 +1,282 @@ +"""polymetis.RobotInterface combined with GripperInterface, with an additional `grasp` method.""" + +import time +import numpy as np +import logging +from scipy.spatial.transform import Rotation as R +import torch + +import hydra +import graspnetAPI +import polymetis + +import ikpy.chain +import tempfile + +log = logging.getLogger(__name__) + +threshold_dist_to_ref_angle = 1.7 + + +def compute_des_pose(best_grasp): + """Convert between GraspNet coordinates to robot coordinates.""" + + # Grasp point + grasp_point = best_grasp.translation + + # Compute plane of rotation through three orthogonal vectors on plane of rotation + grasp_approach_delta = best_grasp.rotation_matrix @ np.array([-0.3, 0.0, 0]) + grasp_approach_delta_plus = best_grasp.rotation_matrix @ np.array([-0.3, 0.1, 0]) + grasp_approach_delta_minus = best_grasp.rotation_matrix @ np.array([-0.3, -0.1, 0]) + bx = -grasp_approach_delta + by = grasp_approach_delta_plus - grasp_approach_delta_minus + bx = bx / np.linalg.norm(bx) + by = by / np.linalg.norm(by) + bz = np.cross(bx, by) + plane_rot = R.from_matrix(np.vstack([bx, by, bz]).T) + + # Convert between GraspNet neutral orientation to robot neutral orientation + des_ori = plane_rot * R.from_euler("y", 90, degrees=True) + des_ori_quat = des_ori.as_quat() + + return grasp_point, grasp_approach_delta, des_ori_quat + + +def grasp_to_pose(grasp: graspnetAPI.Grasp): + return grasp.translation, R.from_matrix(grasp.rotation_matrix).as_quat() + + +def compute_quat_dist(a, b): + return torch.acos((2 * (a * b).sum() ** 2 - 1).clip(-1, 1)) + + +def min_dist_grasp(default_ee_quat, grasps): + """Find the grasp with minimum orientation distance to the reference grasp""" + with torch.no_grad(): + rots_as_quat = [ + torch.Tensor(R.from_matrix(grasp.rotation_matrix).as_quat()) + for grasp in grasps + ] + dists = [compute_quat_dist(rot, default_ee_quat) for rot in rots_as_quat] + i = torch.argmin(torch.Tensor(dists)).item() + log.info(f"Grasp {i} has angle {dists[i]} from reference.") + return grasps[i], i + + +def min_dist_grasp_no_z(default_ee_quat, grasps): + """ + Find the grasp with minimum orientation distance to the reference grasp + disregarding orientation about z axis + """ + with torch.no_grad(): + rots_as_R = [R.from_quat(compute_des_pose(grasp)[2]) for grasp in grasps] + default_r = R.from_quat(default_ee_quat) + dists = [ + np.linalg.norm((rot.inv() * default_r).as_rotvec()[:2]) for rot in rots_as_R + ] + i = torch.argmin(torch.Tensor(dists)).item() + print(f"Grasp {i} has angle {dists[i]} from reference.") + log.info(f"Grasp {i} has angle {dists[i]} from reference.") + return grasps[i], i, dists + + +class GraspingRobotInterface(polymetis.RobotInterface): + def __init__( + self, + gripper: polymetis.GripperInterface, + k_approach=1.5, + k_grasp=0.72, + gripper_max_width=0.085, + # ikpy params: + base_elements=["panda_link0"], + # soft_limits=[ + # (-2.70, 2.70), + # (-1.56, 1.56), + # (-2.7, 2.7), + # (-2.87, -0.07), + # (-2.7, 2.7), + # (-0.02, 3.55), + # (-2.7, 2.7), + # ], + soft_limits=[ + (-3.1, 0.5), + (-1.5, 3.1), + (-3.1, 3.1), + (-3.1, 1.5), + (-2.7, 2.7), + (-3.1, 3.55), + (-2.7, 2.7), + ], + *args, + **kwargs, + ): + super().__init__(*args, **kwargs) + self.gripper = hydra.utils.instantiate(gripper) + + self.default_ee_quat = torch.Tensor([1, 0, 0, 0]) + self.k_approach = k_approach + self.k_grasp = k_grasp + self.gripper_max_width = gripper_max_width + + with tempfile.NamedTemporaryFile(mode="w+") as f: + f.write(self.metadata.urdf_file) + f.seek(0) + self.robot_model_ikpy = ikpy.chain.Chain.from_urdf_file( + f.name, + base_elements=base_elements, + ) + for i in range(len(soft_limits)): + self.robot_model_ikpy.links[i + 1].bounds = soft_limits[i] + + def ik(self, position, orientation=None): + curr_joint_pos = [0] + self.get_joint_positions().numpy().tolist() + [0] + des_homog_transform = np.eye(4) + if orientation is not None: + des_homog_transform[:3, :3] = R.from_quat(orientation).as_matrix() + des_homog_transform[:3, 3] = position + try: + joint_pos_ikpy = self.robot_model_ikpy.inverse_kinematics_frame( + target=des_homog_transform, + orientation_mode="all", + no_position=False, + initial_position=curr_joint_pos, + ) + return joint_pos_ikpy[1:-1] + except ValueError as e: + print(f"Can't find IK solution! {e}") + return None + + def gripper_open(self): + self.gripper.goto(1, 1, 1) + + def gripper_close(self): + self.gripper.goto(0, 1, 1) + + def move_until_success( + self, position, orientation, time_to_go, max_attempts=2, success_dist=0.05 + ): + states = [] + for _ in range(max_attempts): + joint_pos = self.ik(position, orientation) + if joint_pos is None: + print('going home as no solution was found for {position} with IK') + self.go_home() + states += self.move_to_joint_positions(joint_pos, time_to_go=time_to_go) + curr_ee_pos, curr_ee_ori = self.get_ee_pose() + + xyz_diff = torch.linalg.norm(curr_ee_pos - position) + ori_diff = ( + R.from_quat(curr_ee_ori).inv() * R.from_quat(orientation) + ).magnitude() + log.info(f"Dist to goal: xyz diff {xyz_diff}, ori diff {ori_diff}") + + if ( + states + and states[-1].prev_command_successful + and xyz_diff < success_dist + and ori_diff < 0.2 + ): + break + return states, curr_ee_pos + + def check_feasibility(self, point: np.ndarray): + return self.ik(point) is not None + + def select_grasp( + self, grasps: graspnetAPI.GraspGroup, num_grasp_choices=150 + ) -> graspnetAPI.Grasp: + with torch.no_grad(): + feasible_i = [] + for i, grasp in enumerate(grasps): + print(f"checking feasibility {i}/{len(grasps)}") + + if grasp.width > self.gripper_max_width: + continue + + grasp_point, grasp_approach_delta, des_ori_quat = compute_des_pose( + grasp + ) + # Vidhi: check with Yixin + # breakpoint() + point_a = grasp_point + self.k_approach * grasp_approach_delta + point_b = grasp_point + self.k_grasp * grasp_approach_delta + + #Vidhi : maybe change z of these points + + if self.check_feasibility(point_a) and self.check_feasibility(point_b): + feasible_i.append(i) + + if len(feasible_i) == num_grasp_choices: + if i >= num_grasp_choices: + print( + f"Kinematically filtered {i + 1 - num_grasp_choices} grasps" + " to get feasible positions" + ) + break + + # Choose the grasp closest to the neutral position + filtered_grasps = grasps[feasible_i] + if len(filtered_grasps) > 1: + grasp, i, dists = min_dist_grasp_no_z(self.default_ee_quat, filtered_grasps) + if dists[i] < threshold_dist_to_ref_angle: + log.info(f"Closest grasp to ee ori, within top {len(grasps)}: {i + 1}") + # breakpoint() + return grasp, filtered_grasps + else: + print('angle too big, will hit the table!!!') + + return None, filtered_grasps + + + def grasp( + self, + grasp: graspnetAPI.Grasp, + time_to_go=3, + gripper_width_success_threshold=0.00095, + ): + states = [] + grasp_point, grasp_approach_delta, des_ori_quat = compute_des_pose(grasp) + + self.gripper_open() + approach_pos = torch.Tensor(grasp_point + grasp_approach_delta * self.k_approach) + states += self.move_until_success( + position=approach_pos, + orientation=torch.Tensor(des_ori_quat), + time_to_go=time_to_go, + ) + curr_ee_pos, curr_ee_ori = self.get_ee_pose() + if torch.linalg.norm(curr_ee_pos - approach_pos) > 15e-2: + success = False + return states, success + + # if states[-1] + # except: + # print('Exception : failed to execute the grasp approach') + # success = False + # return states, success + grip_ee_pos = torch.Tensor(grasp_point + grasp_approach_delta * self.k_grasp) - torch.Tensor([0, 0, 0.02]) + + + states += self.move_until_success( + position=grip_ee_pos, + orientation=torch.Tensor(des_ori_quat), + time_to_go=time_to_go, + ) + + if torch.linalg.norm(curr_ee_pos - grip_ee_pos) > 15e-2: + success = False + return states, success + + self.gripper_close() + + log.info(f"Waiting for gripper to close...") + while self.gripper.get_state().is_moving: + time.sleep(0.2) + + gripper_state = self.gripper.get_state() + width = gripper_state.width + log.info(f"Gripper width: {width}") + + success = width > gripper_width_success_threshold + + return states, success diff --git a/perception/sandbox/polygrasp/src/polygrasp/segmentation_rpc.py b/perception/sandbox/polygrasp/src/polygrasp/segmentation_rpc.py new file mode 100644 index 0000000000..40b844ca41 --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp/segmentation_rpc.py @@ -0,0 +1,56 @@ +import logging +import a0 +import numpy as np +import signal + +from polygrasp import serdes + +topic_key = "segmentation" + +log = logging.getLogger(__name__) + + +class SegmentationClient: + def __init__(self): + self.client = a0.RpcClient(topic_key) + + def segment_img(self, rgbd, min_mask_size=2500): + bits = serdes.rgbd_to_capnp(rgbd).to_bytes() + result_bits = self.client.send_blocking(bits).payload + labels = serdes.capnp_to_rgbd(result_bits) + + num_objs = int(labels.max()) + obj_masked_rgbds = [] + obj_masks = [] + + for obj_i in range(1, num_objs + 1): + obj_mask = labels == obj_i + + obj_mask_size = obj_mask.sum() + if obj_mask_size < min_mask_size: + continue + obj_masked_rgbd = rgbd * obj_mask[:, :, None] + obj_masked_rgbds.append(obj_masked_rgbd) + obj_masks.append(obj_mask) + + return obj_masked_rgbds, obj_masks + + +class SegmentationServer: + def _get_segmentations(self, rgbd: np.ndarray): + raise NotImplementedError + + def start(self): + def onrequest(req): + log.info("Got request; computing segmentations...") + + payload = req.pkt.payload + rgbd = serdes.capnp_to_rgbd(payload) + result = self._get_segmentations(rgbd) + + log.info("Done. Replying with serialized segmentations...") + req.reply(serdes.rgbd_to_capnp(result).to_bytes()) + + server = a0.RpcServer(topic_key, onrequest, None) + log.info("Starting server...") + signal.pause() diff --git a/perception/sandbox/polygrasp/src/polygrasp/serdes.py b/perception/sandbox/polygrasp/src/polygrasp/serdes.py new file mode 100644 index 0000000000..d49b2464fd --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp/serdes.py @@ -0,0 +1,116 @@ +import numpy as np +import open3d as o3d +import fairomsg + +import site +import os +import io +import numpy as np +import open3d +import graspnetAPI +import capnp +import polygrasp + +import cv2 + +std_msgs = fairomsg.get_msgs("std_msgs") +sensor_msgs = fairomsg.get_msgs("sensor_msgs") +geometry_msgs = fairomsg.get_msgs("geometry_msgs") + +print("capnp loading polygrasp") +_schema_parser = capnp.SchemaParser() +polygrasp_msgs = _schema_parser.load( + os.path.join(polygrasp.__path__[0], "polygrasp.capnp"), + imports=site.getsitepackages(), +) + +"""Byte conversions""" + + +def np_to_bytes(arr: np.ndarray): + with io.BytesIO() as f: + np.save(f, arr) + return f.getvalue() + + +def bytes_to_np(bytes_arr: bytes): + with io.BytesIO(bytes_arr) as f: + return np.load(f) + + +def grasp_group_to_bytes(grasp_group: graspnetAPI.grasp.GraspGroup): + return np_to_bytes(grasp_group.grasp_group_array) + + +def bytes_to_grasp_group(arr_bytes: bytes): + return graspnetAPI.grasp.GraspGroup(bytes_to_np(arr_bytes)) + + +def open3d_pcd_to_bytes(cloud: open3d.geometry.PointCloud): + if cloud.has_colors(): + arr = np.hstack([np.asarray(cloud.points), np.asarray(cloud.colors)]) + else: + arr = np.asarray(cloud.points) + return np_to_bytes(arr) + + +def bytes_to_open3d_pcd(arr_bytes: bytes): + arr = bytes_to_np(arr_bytes) + result = open3d.geometry.PointCloud( + open3d.cuda.pybind.utility.Vector3dVector(arr[:, :3]) + ) + if arr.shape[1] == 6: + result.colors = open3d.cuda.pybind.utility.Vector3dVector(arr[:, 3:]) + + return result + + +"""Capnp conversions""" + + +def pcd_to_capnp(pcd: o3d.geometry.PointCloud): + result = sensor_msgs.PointCloud2() + result.data = open3d_pcd_to_bytes(pcd) + return result + + +def capnp_to_pcd(blob): + capnp_pcd = sensor_msgs.PointCloud2.from_bytes(blob) + return bytes_to_open3d_pcd(capnp_pcd.data) + + +def grasp_group_to_capnp(grasp_group: graspnetAPI.grasp.GraspGroup): + capnp_gg = std_msgs.ByteMultiArray() + capnp_gg.data = grasp_group_to_bytes(grasp_group) + return capnp_gg + + +def capnp_to_grasp_group(blob): + capnp_gg = std_msgs.ByteMultiArray.from_bytes(blob) + + gg = bytes_to_grasp_group(capnp_gg.data) + return gg + + +def rgbd_to_capnp(rgbd): + img = sensor_msgs.Image() + img.data = np_to_bytes(rgbd) + + return img + + +def capnp_to_rgbd(blob): + img = sensor_msgs.Image.from_bytes(blob) + return bytes_to_np(img.data) + + +def load_bw_img(path): + grayscale_img = cv2.imread(path, cv2.IMREAD_GRAYSCALE) + thresh, bw_img = cv2.threshold( + grayscale_img, 128, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU + ) + return bw_img.astype(bool) + + +def save_bw_img(img, name): + cv2.imwrite(f"{name}.png", img * 255) diff --git a/perception/sandbox/polygrasp/src/polygrasp_test/__init__.py b/perception/sandbox/polygrasp/src/polygrasp_test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/perception/sandbox/polygrasp/src/polygrasp_test/mocked.py b/perception/sandbox/polygrasp/src/polygrasp_test/mocked.py new file mode 100644 index 0000000000..90c1f3008d --- /dev/null +++ b/perception/sandbox/polygrasp/src/polygrasp_test/mocked.py @@ -0,0 +1,40 @@ +import hydra +import numpy as np +import torch + +from polygrasp.cam_pub_sub import PointCloudSubscriber + + +class MockedGraspingRobotInterface: + def __init__(self): + pass + + def gripper_open(self, *args, **kwargs): + pass + + def gripper_close(self, *args, **kwargs): + pass + + def go_home(self, *args, **kwargs): + pass + + def move_until_success(self, *args, **kwargs): + return [] + + def grasp(self, *args, **kwargs): + return [], True + + def get_ee_pose(self): + return torch.zeros(3), None + + def select_grasp(self, grasp_group): + return grasp_group[0], grasp_group + + +class MockedCam(PointCloudSubscriber): + def __init__(self, rgbd_path, *args, **kwargs): + super().__init__(*args, **kwargs) + self.rgbd_img = np.load(hydra.utils.to_absolute_path(rgbd_path)) + + def get_rgbd(self): + return self.rgbd_img diff --git a/perception/sandbox/polygrasp/third_party/UnseenObjectClustering b/perception/sandbox/polygrasp/third_party/UnseenObjectClustering new file mode 160000 index 0000000000..9d08e0accd --- /dev/null +++ b/perception/sandbox/polygrasp/third_party/UnseenObjectClustering @@ -0,0 +1 @@ +Subproject commit 9d08e0accdfa0fafe571aa40f1ec0c648348b334 diff --git a/perception/sandbox/polygrasp/third_party/graspnet-baseline b/perception/sandbox/polygrasp/third_party/graspnet-baseline new file mode 160000 index 0000000000..3fc8c0b289 --- /dev/null +++ b/perception/sandbox/polygrasp/third_party/graspnet-baseline @@ -0,0 +1 @@ +Subproject commit 3fc8c0b289da6b81aa26431bca9185813eef328f diff --git a/perception/sandbox/vj_franka_hardware.yaml b/perception/sandbox/vj_franka_hardware.yaml new file mode 100644 index 0000000000..53d30e8dc7 --- /dev/null +++ b/perception/sandbox/vj_franka_hardware.yaml @@ -0,0 +1,92 @@ +hz: 1000 +use_real_time: true +exec: franka_panda_client + +robot_client: + _target_: polymetis.robot_client.executable_robot_client.ExecutableRobotClient + use_real_time: ${use_real_time} + metadata_cfg: + _target_: polymetis.robot_client.metadata.RobotClientMetadata + default_Kq: [80, 120, 100, 100, 70, 50, 20] + default_Kqd: [10, 10, 10, 10, 5, 5, 5] + default_Kx: [150, 150, 150, 10, 10, 10] + default_Kxd: [25, 25, 25, 7, 7, 7] + hz: ${hz} + robot_model_cfg: ${robot_model} + executable_cfg: + robot_ip: "172.16.0.2" + control_ip: ${ip} + control_port: ${port} + readonly: false + mock: false + use_real_time: ${use_real_time} + hz: ${hz} + num_dofs: ${robot_model.num_dofs} + exec: ${exec} + robot_client_metadata_path: ??? + + limit_rate: true + lpf_cutoff_frequency: 100 + + limits: + # bounding box of the workspace + cartesian_pos_upper: + - 1.0 + - 0.8 + - 1.0 + cartesian_pos_lower: + - -0.5 + - -0.8 + - -0.5 + + # the remaining limits are set to the original franka limits minus a margin + joint_pos_upper: #margin: 0.1 rad + - 3.1 + - 3.1 + - 3.1 + - 1.5 + - 3.1 + - 3.65 + - 3.1 + joint_pos_lower: #margin: 0.1 rad + - -3.1 + - -3.1 + - -3.1 + - -3.1 + - -3.1 + - -3.1 + - -3.1 + joint_vel: #margin: 0.1 rad/s + - 1.5 + - 1.5 + - 1.5 + - 1.5 + - 2.51 + - 2.51 + - 2.51 + elbow_vel: 1.075 #margin: 0.1 rad/s + joint_torques: #margin: 1N for first 4 joints, 0.5N for last 3 joints + - 86.0 + - 86.0 + - 86.0 + - 86.0 + - 11.5 + - 11.5 + - 11.5 + + collision_behavior: + lower_torque: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0] + upper_torque: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0] + lower_force: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0] + upper_force: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0] + + safety_controller: + is_active: true + margins: # margin from hard safety limits at which safety controllers start to kick in + cartesian_pos: 0.05 + joint_pos: 0.2 + joint_vel: 0.5 + stiffness: + cartesian_pos: 200.0 + joint_pos: 50.0 + joint_vel: 20.0 diff --git a/perception/sandbox/vj_handcal_collect_grasp.sh b/perception/sandbox/vj_handcal_collect_grasp.sh new file mode 100644 index 0000000000..85b1a6090b --- /dev/null +++ b/perception/sandbox/vj_handcal_collect_grasp.sh @@ -0,0 +1,2 @@ +# record cal +# calibration points diff --git a/tools/docker/Dockerfile b/tools/docker/Dockerfile index 0932debb4b..163f53149c 100644 --- a/tools/docker/Dockerfile +++ b/tools/docker/Dockerfile @@ -6,7 +6,7 @@ ARG AWS_ACCESS_KEY_ID ARG AWS_SECRET_ACCESS_KEY # Clone/make repo -RUN time git clone --recursive --shallow-submodules --depth 1 --branch ${current_branch} --jobs 5 https://github.com/facebookresearch/fairo +RUN time git clone --recurse-submodules="./droidlet" --shallow-submodules --depth 1 --branch ${current_branch} --jobs 5 https://github.com/facebookresearch/fairo WORKDIR fairo RUN pip3 install -r requirements.txt