From d12c578f8f4e8085f49eda9b7a812d315e467925 Mon Sep 17 00:00:00 2001 From: StoneT2000 Date: Sun, 28 Jan 2024 23:38:31 -0800 Subject: [PATCH] fix collision setting bugs --- mani_skill2/agents/robots/fetch/fetch.py | 9 +++---- mani_skill2/envs/tasks/open_cabinet_drawer.py | 24 +++++++++---------- .../table/table_scene_builder.py | 2 +- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/mani_skill2/agents/robots/fetch/fetch.py b/mani_skill2/agents/robots/fetch/fetch.py index 3f4f1358f..fa9af35c1 100644 --- 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 index 05b1d3a20..32fb7bb53 100644 --- 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 index d09adbb20..3e7dc7ebb 100644 --- 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)