added concave flag to container

cfg/active_grasp.rviz

@@ -254,7 +254,10 @@ Visualization Manager:
       Marker Topic: visualization_marker_array
       Name: Markers
       Namespaces:
-        {}
+        bbox: true
+        path: true
+        roi: true
+        views: true
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -385,25 +388,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.376084327697754
+      Distance: 1.8333022594451904
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Field of View: 0.7799999713897705
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.23980742692947388
-        Y: 0.16715103387832642
-        Z: 0.4434542953968048
+        X: 0.18340304493904114
+        Y: -0.2598724365234375
+        Z: 0.2933075726032257
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3299999237060547
+      Pitch: 0.4453985095024109
       Target Frame: <Fixed Frame>
-      Yaw: 0.8950039148330688
+      Yaw: 0.6903981566429138
     Saved:
       - Class: rviz/Orbit
         Distance: 1.2000000476837158
@@ -444,4 +447,4 @@ Window Geometry:
     collapsed: true
   Width: 1095
   X: 1232
-  Y: 125
+  Y: 51

cfg/sim/challenging_scene_1.yaml

@@ -1,15 +1,11 @@
 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.1, 0.0, 0.5]
-  rpy: [90, 0, 0]
-  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: 1
+- object_id: ycb/006_mustard_bottle
+  xyz: [0.0, 0.0, 1.6]
+  rpy: [0, 0, 0]
+  scale: 0.8
+- object_id: active_perception/box2
+  xyz: [0.0, 0.0, 1.3]
+  rpy: [0, 0, 0]
+  scale: 0.85

src/active_grasp/active_perception_policy.py

@@ -218,8 +218,8 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
             look_at_center_T[:3, 3] = look_at_center.cpu().numpy()
             look_at_center_T = current_cam_pose.cpu().numpy() @ look_at_center_T
             look_at_center = torch.from_numpy(look_at_center_T[:3, 3]).float().to("cuda:0")
-            print(f"Central Point of Target: {central_point_of_target}, length: {np.linalg.norm(central_point_of_target)}")
-            print(f"camera position: {current_cam_pose[:3, 3]}")
+            # print(f"Central Point of Target: {central_point_of_target}, length: {np.linalg.norm(central_point_of_target)}")
+            # print(f"camera position: {current_cam_pose[:3, 3]}")
             nbv_tensor = self.get_transformed_mat(current_cam_pose, 
                                                   est_delta_rot_mat, 
                                                   look_at_center)

src/active_grasp/simulation.py

@@ -155,10 +155,6 @@ 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