cusotm scene test passed

README.md

@@ -1,5 +1,4 @@
 # Roadmap
-* Debug with model output pose
 * Change simulation scenes
 * Interface with GS-Net
 

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: 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
   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

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

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

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)

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