From 28f67382c767548afb271a394d3ac9d4957370cd Mon Sep 17 00:00:00 2001 From: 0nhc Date: Sat, 19 Oct 2024 13:57:56 -0500 Subject: [PATCH] cusotm scene test passed --- README.md | 1 - cfg/active_grasp.rviz | 12 ++++++------ cfg/active_grasp.yaml | 4 ++-- cfg/sim/challenging_scene_1.yaml | 16 ++++++++++------ scripts/run.py | 2 +- src/active_grasp/simulation.py | 17 ++++++++++++++++- 6 files changed, 35 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index 2544281..af762ca 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,4 @@ # Roadmap -* Debug with model output pose * Change simulation scenes * Interface with GS-Net diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index c3d16ff..53b456c 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -7,7 +7,7 @@ Panels: - /TF1/Frames1 - /Markers1/Namespaces1 Splitter Ratio: 0.6881287693977356 - Tree Height: 104 + Tree Height: 181 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -428,12 +428,12 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 612 + Height: 778 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 000000ff00000000fd0000000400000000000001db000002b0fc0200000017fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000138000000c900fffffffa000000000100000002fb000000100044006900730070006c0061007900730100000000000001f30000015600fffffffb000000140043006f006c006f00720049006d0061006700650000000000ffffffff0000000000000000fb000000140043006f006c006f00720049006d00610067006501000001e4000001bf0000000000000000fb00000014004400650070007400680049006d006100670065000000029a000001090000000000000000fb000000140043006f006c006f00720049006d00610067006501000001d2000001920000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680000000166000000b50000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001f4000001af0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c00460069006c0074006500720065006400200043006c006f00750064000000029a000001050000000000000000fb000000140043006f006c006f00720049006d006100670065010000019f000002040000000000000000fb0000000a0049006d006100670065010000017b000001720000001600fffffffb0000000a0049006d006100670065000000021c000000ee0000000000000000fb0000000c00430061006d00650072006100000002db000000c80000000000000000fb000000140043006f006c006f00720049006d006100670065010000019f000002040000000000000000fb0000000a0049006d006100670065010000019c000002070000000000000000fb0000000a0049006d0061006700650100000192000001b80000000000000000000000010000017b00000366fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000366000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000002fb000000140049006e0066007200610049006d00610067006502000000000000006b00000280000001e0fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056300000039fc0100000002fb0000000800540069006d00650000000000000005630000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000266000002b000000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -442,6 +442,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1244 - X: 1184 - Y: 195 + Width: 1095 + X: 1232 + Y: 125 diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index f96a272..dfe9b55 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -1,8 +1,8 @@ bt_sim: gui: True gripper_force: 10 - scene: random - # scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml + # scene: random + scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml hw: roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt diff --git a/cfg/sim/challenging_scene_1.yaml b/cfg/sim/challenging_scene_1.yaml index 3a270bf..d8b662e 100644 --- a/cfg/sim/challenging_scene_1.yaml +++ b/cfg/sim/challenging_scene_1.yaml @@ -1,11 +1,15 @@ -center: [0.5, 0.2, 0.25] +center: [0.5, 0.0, 0.25] q: [0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79] objects: - object_id: ap_dataset/objects/apple - xyz: [0.0, 0.0, 0.2] + xyz: [0.1, 0.0, 0.5] rpy: [90, 0, 0] - scale: 0.001 -- object_id: ap_dataset/occluders/flower_pot_021 - xyz: [0.0, 0.0, 0.2] + scale: 1 +# - object_id: ycb/006_mustard_bottle +# xyz: [0.0, 0.0, 0.2] +# rpy: [0, 0, 0] +# scale: 1 +- object_id: ap_dataset/occluders/blender_box_001 + xyz: [0.0, 0.0, 0.15] rpy: [90, 0, 0] - scale: 0.001 + scale: 1 diff --git a/scripts/run.py b/scripts/run.py index 9c820d0..d072912 100755 --- a/scripts/run.py +++ b/scripts/run.py @@ -46,7 +46,7 @@ def main(): def create_parser(): parser = argparse.ArgumentParser() parser.add_argument("policy", type=str, choices=registry.keys()) - parser.add_argument("--runs", type=int, default=20) + parser.add_argument("--runs", type=int, default=10) parser.add_argument("--wait-for-input", action="store_true") parser.add_argument("--logdir", type=Path, default="logs") parser.add_argument("--seed", type=int, default=1) diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index 48d67cb..9cfa304 100644 --- a/src/active_grasp/simulation.py +++ b/src/active_grasp/simulation.py @@ -58,6 +58,13 @@ class Simulation: self.vgn = VGN(model_path) def reset(self): + # Wait until the simulation is stable + secs = 3.0 + sleep_ticks = self.rate * secs + for i in range(int(sleep_ticks)): + p.stepSimulation() + + # Reset the scene valid = False while not valid: self.set_arm_configuration([0.0, -1.39, 0.0, -2.36, 0.0, 1.57, 0.79]) @@ -66,7 +73,8 @@ class Simulation: self.set_arm_configuration(q) uid = self.select_target() bbox = self.get_target_bbox(uid) - valid = self.check_for_grasps(bbox) + valid = True + # valid = self.check_for_grasps(bbox) return bbox def set_arm_configuration(self, q): @@ -81,6 +89,9 @@ class Simulation: uids, counts = np.unique(mask, return_counts=True) mask = np.isin(uids, self.scene.object_uids) # remove ids of the floor, etc uids, counts = uids[mask], counts[mask] + # If no objects are detected, try again + if(len(uids) == 0 and len(counts) == 0): + return self.select_target() target_uid = uids[np.argmin(counts)] p.changeVisualShape(target_uid, -1, rgbaColor=[1, 0, 0, 1]) return target_uid @@ -144,6 +155,10 @@ class Scene: def add_object(self, urdf, ori, pos, scale=1.0): uid = p.loadURDF(str(urdf), pos, ori.as_quat(), globalScaling=scale) + collision_shape_data = p.getCollisionShapeData(uid, -1) + for shape in collision_shape_data: + shape_id = shape[1] # Get the shape ID + p.changeDynamics(uid, shape_id, collisionMargin=0.0001) # Set collision margin to 0.01 self.object_uids.append(uid) return uid