diff --git a/doc b/doc index d92ab5c51f..8764c4c5ac 160000 --- a/doc +++ b/doc @@ -1 +1 @@ -Subproject commit d92ab5c51f12d898a522bb0fa814ba56b3ed3181 +Subproject commit 8764c4c5ac37ab3e287997adf405a06abd10a4cc diff --git a/genesis/engine/scene.py b/genesis/engine/scene.py index 9b179fe163..0715f59ba8 100644 --- a/genesis/engine/scene.py +++ b/genesis/engine/scene.py @@ -42,6 +42,7 @@ from genesis.utils.misc import tensor_to_array, sanitize_index from genesis.vis import Visualizer from genesis.utils.warnings import warn_once +import genesis.utils.mesh as mu if TYPE_CHECKING: from genesis.engine.entities.base_entity import Entity @@ -1250,6 +1251,54 @@ def draw_debug_points(self, poss, colors=(1.0, 0.0, 0.0, 0.5)): with self._visualizer.viewer_lock: return self._visualizer.context.draw_debug_points(poss, colors) + @gs.assert_built + def draw_debug_frustum(self, camera, color=(1.0, 1.0, 1.0, 0.3)): + """ + Draws a camera frustum in the scene for visualization. + + Parameters + ---------- + camera : Camera + The camera object whose frustum will be visualized. Works for any + camera including sensor cameras. + color : array_like, shape (4,), optional + The color of the frustum in RGBA format. + + Returns + ------- + node : genesis.ext.pyrender.mesh.Mesh + The created debug object. + """ + with self._visualizer.viewer_lock: + mesh = mu.create_camera_frustum(camera, color) + return self._visualizer.context.draw_debug_mesh(mesh) + + @gs.assert_built + def draw_debug_trajectory(self, poss, radius=0.002, color=(1.0, 0.5, 0.0, 0.8)): + """ + Draws a trajectory as a series of connected lines in the scene for visualization. + + Parameters + ---------- + poss : array_like, shape (N, 3) + The positions of the trajectory points. + radius : float, optional + The radius of the trajectory lines. + color : array_like, shape (4,), optional + The color of the trajectory in RGBA format. + + Returns + ------- + nodes : list + List of created debug line objects. + """ + nodes = [] + with self._visualizer.viewer_lock: + for i in range(len(poss) - 1): + node = self._visualizer.context.draw_debug_line(poss[i], poss[i + 1], radius, color) + nodes.append(node) + return nodes + @gs.assert_built def draw_debug_path(self, qposs, entity, link_idx=-1, density=0.3, frame_scaling=1.0): """ diff --git a/tests/test_render.py b/tests/test_render.py index cae1f06ea4..c36e080edc 100644 --- a/tests/test_render.py +++ b/tests/test_render.py @@ -1041,6 +1041,70 @@ def test_draw_debug(renderer, show_viewer): assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS) +@pytest.mark.required +@pytest.mark.parametrize("renderer_type", [RENDERER_TYPE.RASTERIZER]) +def test_draw_debug_frustum_and_trajectory(renderer, show_viewer): + """Test that draw_debug_frustum and draw_debug_trajectory render visible content.""" + if "GS_DISABLE_OFFSCREEN_MARKERS" in os.environ: + pytest.skip("Offscreen rendering of markers is forcibly disabled. Skipping...") + + scene = gs.Scene( + renderer=renderer, + show_viewer=show_viewer, + show_FPS=False, + ) + cam = scene.add_camera( + pos=(3.5, 0.5, 2.5), + lookat=(0.0, 0.0, 0.5), + res=(640, 640), + debug=True, + GUI=show_viewer, + ) + # Add sensor_cam BEFORE build + sensor_cam = scene.add_camera( + res=(640, 480), + pos=(1.0, 0.0, 1.0), + lookat=(0.0, 0.0, 0.0), + fov=45, + GUI=False, + ) + scene.build() + + # Verify scene is initially blank + rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False) + assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS) + + # Test draw_debug_frustum + scene.draw_debug_frustum(sensor_cam, color=(0.0, 1.0, 0.0, 0.5)) + scene.visualizer.update() + + rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False) + rgb_array_flat = rgb_array.reshape((-1, 3)).astype(np.int32) + assert (np.std(rgb_array_flat, axis=0) > 10.0).any() + + # Clear and verify blank again + scene.clear_debug_objects() + scene.visualizer.update() + rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False) + assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS) + + # Test draw_debug_trajectory + t = np.linspace(0, 2 * np.pi, 50) + positions = np.column_stack([np.cos(t), np.sin(t), np.ones_like(t) * 0.5]) + scene.draw_debug_trajectory(positions, radius=0.01, color=(1.0, 0.5, 0.0, 1.0)) + scene.visualizer.update() + + rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False) + rgb_array_flat = rgb_array.reshape((-1, 3)).astype(np.int32) + assert (np.std(rgb_array_flat, axis=0) > 10.0).any() + + # Clear and verify blank again + scene.clear_debug_objects() + scene.visualizer.update() + rgb_array, *_ = cam.render(rgb=True, depth=False, segmentation=False, colorize_seg=False, normal=False) + assert_allclose(np.std(rgb_array.reshape((-1, 3)), axis=0), 0.0, tol=gs.EPS) + + @pytest.mark.slow # ~150s @pytest.mark.required @pytest.mark.parametrize("n_envs", [0, 2])