support collision body visualization for rigid object and articulation

embodichain/lab/sim/objects/articulation.py

@@ -579,6 +579,11 @@ def __init__(
         # set default collision filter
         self._set_default_collision_filter()
 
+        # flag for collision visible node existence
+        self._has_collision_visible_node_dict = dict()
+        for link_name in self.link_names:
+            self._has_collision_visible_node_dict[link_name] = False
+
     def __str__(self) -> str:
         parent_str = super().__str__()
         return parent_str + f" | dof: {self.dof} | num_links: {self.num_links}"
@@ -1535,6 +1540,64 @@ def get_visual_material_inst(
                 result.append(mat_dict)
         return result
 
+    def set_physical_visible(
+        self,
+        visible: bool = True,
+        link_names: Optional[List[str]] = None,
+        rgba: Optional[Sequence[float]] = None,
+    ):
+        """set collision
+
+        Args:
+            visible (bool, optional): is collision body visible. Defaults to True.
+            link_names (Optional[List[str]], optional): links to set visibility. Defaults to None.
+            rgba (Optional[Sequence[float]], optional): collision body visible rgba. It will be defined at the first time the function is called. Defaults to None.
+        """
+        rgba = rgba if rgba is not None else (0.8, 0.2, 0.2, 0.7)
+        if len(rgba) != 4:
+            logger.log_error(f"Invalid rgba {rgba}, should be a sequence of 4 floats.")
+        rgba = np.array(
+            [
+                rgba[0],
+                rgba[1],
+                rgba[2],
+                rgba[3],
+            ]
+        )
+        link_names = self.link_names if link_names is None else link_names
+
+        # create collision visible node if not exist
+        if visible:
+            for i, env_idx in enumerate(self._all_indices):
+                for link_name in link_names:
+                    if self._has_collision_visible_node_dict[link_name] is False:
+                        self._entities[env_idx].create_physical_visible_node(
+                            rgba, link_name
+                        )
+                        self._has_collision_visible_node_dict[link_name] = True
+
+        # set visibility
+        for i, env_idx in enumerate(self._all_indices):
+            for link_name in link_names:
+                self._entities[env_idx].set_physical_visible(visible, link_name)
+
+    def set_visible(
+        self,
+        visible: bool = True,
+        link_names: Optional[List[str]] = None,
+    ) -> None:
+        """Set the visibility of the robot or a specific control part.
+
+        Args:
+            visible (bool, optional): Whether the robot or control part is visible. Defaults to True.
+            link_names (Optional[List[str]], optional): links to set visibility. Defaults to None.
+        """
+        link_names = self.link_names if link_names is None else link_names
+
+        for i, env_idx in enumerate(self._all_indices):
+            for link_name in link_names:
+                self._entities[env_idx].set_visible(visible, link_name)
+
     def destroy(self) -> None:
         env = self._world.get_env()
         arenas = env.get_all_arenas()

embodichain/lab/sim/objects/gizmo.py

@@ -424,7 +424,7 @@ def toggle_visibility(self) -> bool:
 
         return self._is_visible
 
-    def set_visibility(self, visible: bool):
+    def set_visible(self, visible: bool):
         """
         Set the visibility of the gizmo.
 

embodichain/lab/sim/objects/rigid_object.py

@@ -196,6 +196,9 @@ def __init__(
         # set default collision filter
         self._set_default_collision_filter()
 
+        # reserve flag for collision visible node existence
+        self._has_collision_visible_node = False
+
     def __str__(self) -> str:
         parent_str = super().__str__()
         return (
@@ -648,6 +651,50 @@ def clear_dynamics(self, env_ids: Optional[Sequence[int]] = None) -> None:
                 data_type=RigidBodyGPUAPIWriteType.TORQUE,
             )
 
+    def set_physical_visible(
+        self,
+        visible: bool = True,
+        rgba: Optional[Sequence[float]] = None,
+    ):
+        """set collion render visibility
+
+        Args:
+            visible (bool, optional): is collision body visible. Defaults to True.
+            rgba (Optional[Sequence[float]], optional): collision body visible rgba. It will be defined at the first time the function is called. Defaults to None.
+        """
+        rgba = rgba if rgba is not None else (0.8, 0.2, 0.2, 0.7)
+        if len(rgba) != 4:
+            logger.log_error(f"Invalid rgba {rgba}, should be a sequence of 4 floats.")
+
+        # create collision visible node if not exist
+        if visible:
+            if not self._has_collision_visible_node:
+                for i, env_idx in enumerate(self._all_indices):
+                    self._entities[env_idx].create_physical_visible_node(
+                        np.array(
+                            [
+                                rgba[0],
+                                rgba[1],
+                                rgba[2],
+                                rgba[3],
+                            ]
+                        )
+                    )
+                self._has_collision_visible_node = True
+
+        # create collision visible node if not exist
+        for i, env_idx in enumerate(self._all_indices):
+            self._entities[env_idx].set_physical_visible(visible)
+
+    def set_visible(self, visible: bool = True) -> None:
+        """Set the visibility of the rigid object.
+
+        Args:
+            visible (bool, optional): Whether the rigid object is visible. Defaults to True.
+        """
+        for i, env_idx in enumerate(self._all_indices):
+            self._entities[env_idx].set_visible(visible)
+
     def reset(self, env_ids: Optional[Sequence[int]] = None) -> None:
         local_env_ids = self._all_indices if env_ids is None else env_ids
         num_instances = len(local_env_ids)

embodichain/lab/sim/objects/rigid_object_group.py

@@ -204,6 +204,10 @@ def __init__(
         # set default collision filter
         self._set_default_collision_filter()
 
+        # reserve flag for collision visible node existence
+        n_instances = len(self._entities[0])
+        self._has_collision_visible_node_list = [False] * n_instances
+
     def __str__(self) -> str:
         parent_str = super().__str__()
         return (
@@ -503,6 +507,53 @@ def reset(self, env_ids: Optional[Sequence[int]] = None) -> None:
 
         self.clear_dynamics(env_ids=local_env_ids)
 
+    def set_physical_visible(
+        self,
+        visible: bool = True,
+        rgba: Optional[Sequence[float]] = None,
+    ):
+        """set collion render visibility
+
+        Args:
+            visible (bool, optional): is collision body visible. Defaults to True.
+            rgba (Optional[Sequence[float]], optional): collision body visible rgba. It will be defined at the first time the function is called. Defaults to None.
+        """
+        rgba = rgba if rgba is not None else (0.8, 0.2, 0.2, 0.7)
+        if len(rgba) != 4:
+            logger.log_error(f"Invalid rgba {rgba}, should be a sequence of 4 floats.")
+
+        # create collision visible node if not exist
+        if visible:
+            for i, env_idx in enumerate(self._all_indices):
+                for intance_id, entity in enumerate(self._entities[env_idx]):
+                    if not self._has_collision_visible_node_list[intance_id]:
+                        entity.create_physical_visible_node(
+                            np.array(
+                                [
+                                    rgba[0],
+                                    rgba[1],
+                                    rgba[2],
+                                    rgba[3],
+                                ]
+                            )
+                        )
+                        self._has_collision_visible_node_list[intance_id] = True
+
+        # create collision visible node if not exist
+        for i, env_idx in enumerate(self._all_indices):
+            for entity in self._entities[env_idx]:
+                entity.set_physical_visible(visible)
+
+    def set_visible(self, visible: bool = True) -> None:
+        """Set the visibility of the rigid object group.
+
+        Args:
+            visible (bool, optional): Whether the rigid object group is visible. Defaults to True.
+        """
+        for i, env_idx in enumerate(self._all_indices):
+            for entity in self._entities[env_idx]:
+                entity.set_visible(visible)
+
     def destroy(self) -> None:
         env = self._world.get_env()
         arenas = env.get_all_arenas()

embodichain/lab/sim/objects/robot.py

@@ -587,6 +587,24 @@ def get_control_part_base_pose(
             link_name=root_link_name, env_ids=local_env_ids, to_matrix=to_matrix
         )
 
+    def get_control_part_link_names(self, name: Optional[str] = None) -> List[str]:
+        """Get the link names of the control part.
+
+        Args:
+            name (Optional[str]): The name of the control part. If None, return all link names.
+        Returns:
+            List[str]: link names of the control part.
+        """
+        if name is None:
+            return self.link_names
+        if name in self._control_groups:
+            return self._control_groups[name].link_names
+        else:
+            logger.log_warning(
+                f"The control part '{name}' does not exist in the robot's control parts."
+            )
+            return []
+
     def _extract_control_groups(self) -> Dict[str, ControlGroup]:
         r"""Extract control groups from the active joint names.
 
@@ -649,5 +667,61 @@ def build_pk_serial_chain(self) -> None:
         """
         self.pk_serial_chain = self.cfg.build_pk_serial_chain(device=self.device)
 
+    def set_physical_visible(
+        self,
+        visible: bool = True,
+        control_part: Optional[str] = None,
+        rgba: Optional[Sequence[float]] = None,
+    ):
+        """set collision of the robot or a specific control part.
+
+        Args:
+            visible (bool, optional): is collision body visible. Defaults to True.
+            control_part (Optional[str], optional): control part to set visibility. Defaults to None. If None, all links are set.
+            rgba (Optional[Sequence[float]], optional): collision body visible rgba. It will be defined at the first time the function is called. Defaults to None.
+        """
+        rgba = rgba if rgba is not None else (0.8, 0.2, 0.2, 0.7)
+        if len(rgba) != 4:
+            logger.log_error(f"Invalid rgba {rgba}, should be a sequence of 4 floats.")
+        rgba = np.array(
+            [
+                rgba[0],
+                rgba[1],
+                rgba[2],
+                rgba[3],
+            ]
+        )
+        link_names = self.get_control_part_link_names(name=control_part)
+
+        # create collision visible node if not exist
+        if visible:
+            for i, env_idx in enumerate(self._all_indices):
+                for link_name in link_names:
+                    if self._has_collision_visible_node_dict[link_name] is False:
+                        self._entities[env_idx].create_physical_visible_node(
+                            rgba, link_name
+                        )
+                        self._has_collision_visible_node_dict[link_name] = True
+
+        # set visibility
+        for i, env_idx in enumerate(self._all_indices):
+            for link_name in link_names:
+                self._entities[env_idx].set_physical_visible(visible, link_name)
+
+    def set_visible(
+        self, visible: bool = True, control_part: Optional[str] = None
+    ) -> None:
+        """Set the visibility of the robot or a specific control part.
+
+        Args:
+            visible (bool, optional): Whether the robot or control part is visible. Defaults to True.
+            control_part (Optional[str], optional): The name of the control part to set visibility for. If None, all links are set. Defaults to None.
+        """
+        link_names = self.get_control_part_link_names(name=control_part)
+
+        for i, env_idx in enumerate(self._all_indices):
+            for link_name in link_names:
+                self._entities[env_idx].set_visible(visible, link_name)
+
     def destroy(self) -> None:
         return super().destroy()

embodichain/lab/sim/sim_manager.py

@@ -1158,7 +1158,7 @@ def set_gizmo_visibility(
         """
         gizmo = self.get_gizmo(uid, control_part)
         if gizmo is not None:
-            gizmo.set_visibility(visible)
+            gizmo.set_visible(visible)
 
     def add_sensor(self, sensor_cfg: SensorCfg) -> BaseSensor:
         """General interface to add a sensor to the scene and returns a handle.

scripts/tutorials/sim/create_robot.py

@@ -46,7 +46,7 @@ def main():
         description="Create and simulate a robot in SimulationManager"
     )
     parser.add_argument(
-        "--num_envs", type=int, default=1, help="Number of environments to simulate"
+        "--num_envs", type=int, default=4, help="Number of environments to simulate"
     )
     parser.add_argument(
         "--device",

scripts/tutorials/sim/create_scene.py

@@ -24,8 +24,9 @@
 
 from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
 from embodichain.lab.sim.cfg import RigidBodyAttributesCfg
-from embodichain.lab.sim.shapes import CubeCfg
+from embodichain.lab.sim.shapes import CubeCfg, MeshCfg
 from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg
+from dexsim.utility.path import get_resources_data_path
 
 
 def main():

tests/sim/objects/test_articulation.py

@@ -175,6 +175,21 @@ def test_remove_articulation(self):
             self.art.uid not in self.sim.asset_uids
         ), "FAIL: Articulation UID still present after removal"
 
+    def test_set_physical_visible(self):
+        self.art.set_physical_visible(
+            visible=True,
+            rgba=(0.1, 0.1, 0.9, 0.4),
+        )
+        self.art.set_physical_visible(visible=False)
+        all_link_names = self.art.link_names
+        self.art.set_physical_visible(visible=True, link_names=all_link_names[:3])
+
+    def test_set_visible(self):
+        self.art.set_visible(visible=True)
+        self.art.set_visible(visible=False)
+        all_link_names = self.art.link_names
+        self.art.set_visible(visible=True, link_names=all_link_names[:3])
+
     def teardown_method(self):
         """Clean up resources after each test method."""
         self.sim.destroy()

tests/sim/objects/test_rigid_object.py

@@ -67,6 +67,7 @@ def setup_simulation(self, sim_device):
                 uid="table", shape=MeshCfg(fpath=table_path), body_type="static"
             ),
         )
+
         self.chair: RigidObject = self.sim.add_rigid_object(
             cfg=RigidObjectCfg(
                 uid="chair", shape=MeshCfg(fpath=chair_path), body_type="kinematic"
@@ -253,6 +254,18 @@ def test_remove(self):
             self.duck.uid not in self.sim.asset_uids
         ), "Duck UID still present after removal"
 
+    def test_set_physical_visible(self):
+        self.table.set_physical_visible(
+            visible=True,
+            rgba=(0.1, 0.1, 0.9, 0.4),
+        )
+        self.table.set_physical_visible(visible=True)
+        self.table.set_physical_visible(visible=False)
+
+    def test_set_visible(self):
+        self.table.set_visible(visible=True)
+        self.table.set_visible(visible=False)
+
     def teardown_method(self):
         """Clean up resources after each test method."""
         self.sim.destroy()

tests/sim/objects/test_rigid_object_group.py

@@ -107,6 +107,14 @@ def test_remove(self):
             self.obj_group.uid not in self.sim.asset_uids
         ), "Object group UID still present after removal"
 
+    def test_set_physical_visible(self):
+        self.obj_group.set_physical_visible(visible=True)
+        self.obj_group.set_physical_visible(visible=False)
+
+    def test_set_visible(self):
+        self.obj_group.set_visible(visible=True)
+        self.obj_group.set_visible(visible=False)
+
     def teardown_method(self):
         """Clean up resources after each test method."""
         self.sim.destroy()