fix collision setting bugs

haosulab · Jan 29, 2024 · d12c578 · d12c578
1 parent 5ae2332
commit d12c578
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 17 deletions.
diff --git a/mani_skill2/agents/robots/fetch/fetch.py b/mani_skill2/agents/robots/fetch/fetch.py
@@ -315,10 +315,11 @@ def _after_init(self):
             self.robot.get_links(), "r_wheel_link"
         )
         for link in [self.base_link, self.l_wheel_link, self.r_wheel_link]:
-            cs = link._bodies[0].get_collision_shapes()[0]
-            cg = cs.get_collision_groups()
-            cg[2] = FETCH_UNIQUE_COLLISION_BIT
-            cs.set_collision_groups(cg)
+            for body in link._bodies:
+                cs = body.get_collision_shapes()[0]
+                cg = cs.get_collision_groups()
+                cg[2] |= FETCH_UNIQUE_COLLISION_BIT
+                cs.set_collision_groups(cg)
 
         self.queries: Dict[str, Tuple[physx.PhysxGpuContactQuery, Tuple[int]]] = dict()
 

diff --git a/mani_skill2/envs/tasks/open_cabinet_drawer.py b/mani_skill2/envs/tasks/open_cabinet_drawer.py
@@ -63,13 +63,24 @@ def _register_sensors(self):
         ]
 
     def _register_render_cameras(self):
-        pose = look_at(eye=[-1.5, -1.5, 1.5], target=[-0.1, 0, 0.1])
+        pose = look_at(eye=[-2.5, -2.5, 2.5], target=[-0.1, 0, 0.1])
         return CameraConfig("render_camera", pose.p, pose.q, 512, 512, 1, 0.01, 10)
 
     def _load_actors(self):
         self.ground = build_tesselated_square_floor(self._scene)
         self._load_cabinets(["prismatic"])
 
+        from mani_skill2.agents.robots.fetch import FETCH_UNIQUE_COLLISION_BIT
+
+        # TODO (stao) (arth): is there a better way to model robots in sim. This feels very unintuitive.
+        for obj in self.ground._objs:
+            cs = obj.find_component_by_type(
+                sapien.physx.PhysxRigidStaticComponent
+            ).get_collision_shapes()[0]
+            cg = cs.get_collision_groups()
+            cg[2] |= FETCH_UNIQUE_COLLISION_BIT
+            cs.set_collision_groups(cg)
+
     def _load_cabinets(self, joint_types: List[str]):
         rand_idx = torch.randperm(len(self.all_model_ids))
         model_ids = self.all_model_ids[rand_idx]
@@ -171,17 +182,6 @@ def _initialize_actors(self):
                 self.agent.reset(qpos)
                 self.agent.robot.set_pose(sapien.Pose([-1.5, 0, 0]))
 
-                from mani_skill2.agents.robots.fetch import FETCH_UNIQUE_COLLISION_BIT
-
-                # TODO (stao) (arth): is there a better way to model robots in sim. This feels very unintuitive.
-                for obj in self.ground._objs:
-                    cs = obj.find_component_by_type(
-                        sapien.physx.PhysxRigidStaticComponent
-                    ).get_collision_shapes()[0]
-                    cg = cs.get_collision_groups()
-                    cg[2] = FETCH_UNIQUE_COLLISION_BIT
-                    cs.set_collision_groups(cg)
-
     def evaluate(self):
         return {"success": torch.zeros(self.num_envs, device=self.device, dtype=bool)}
 

diff --git a/mani_skill2/utils/scene_builder/table/table_scene_builder.py b/mani_skill2/utils/scene_builder/table/table_scene_builder.py
@@ -120,7 +120,7 @@ def initialize(self):
                 .get_collision_shapes()[0]
             )
             cg = cs.get_collision_groups()
-            cg[2] = FETCH_UNIQUE_COLLISION_BIT
+            cg[2] |= FETCH_UNIQUE_COLLISION_BIT
             cs.set_collision_groups(cg)
         else:
             raise NotImplementedError(self.env.robot_uid)