[FEATURE] Add draw_debug_frustum and draw_debug_trajectory to scene API

[Mehak261124]

Description

Added two new debug visualization methods to scene.py:

• draw_debug_frustum(camera): Visualizes the frustum of any camera including sensor cameras, by reusing the existing create_camera_frustum() from utils/mesh.py. Previously this was only available for the interactive viewer camera via show_cameras=True in vis_options.

• draw_debug_trajectory(positions): Draws a trajectory as a series of connected lines by connecting a list of 3D positions. Useful for visualizing robot paths and motion history.

Both methods follow the same pattern as existing draw_debug_* methods and render as markers — visible in the interactive viewer but invisible to robot cameras (depth, segmentation, RGB renders are unaffected).

Related Issue

Resolves #1049

Motivation and Context

draw_debug_frustum reuses the existing create_camera_frustum() from utils/mesh.py which was previously only used internally for the viewer camera. This change makes frustum visualization available for any camera including sensor cameras, without duplicating code.

draw_debug_trajectory was missing entirely from the debug visualization API and is a commonly needed tool for visualizing robot paths during development and debugging.

How Has This Been / Can This Be Tested?

Tested manually on macOS Apple Silicon (M2), Python 3.11.

Test draw_debug_frustum:

import genesis as gs

gs.init(backend=gs.cpu)
scene = gs.Scene(show_viewer=True)
scene.add_entity(gs.morphs.Plane())
cam = scene.add_camera(res=(640, 480), pos=(3.5, 0.0, 2.5), lookat=(0, 0, 0.5), fov=30, GUI=False)
scene.build()
scene.draw_debug_frustum(cam, color=(0.0, 1.0, 0.0, 0.3))
while scene.viewer.is_alive():
    scene.step()

Test draw_debug_trajectory:

import genesis as gs
import numpy as np

gs.init(backend=gs.cpu)
scene = gs.Scene(show_viewer=True)
scene.add_entity(gs.morphs.Plane())
scene.build()
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.005, color=(1.0, 0.5, 0.0, 1.0))
while scene.viewer.is_alive():
    scene.step()

Screenshots:

Checklist:

• I read the CONTRIBUTING document
• I followed the Submitting Code Changes section
• I tagged the title correctly
• I updated the documentation accordingly
• I tested my changes and added instructions on how to test it for reviewers

[duburcqa]

Remove this.

[duburcqa]

All PRs adding features or fixing bugs must come with accompanying unit tests. In this case you have no other choice than resorting on screenshot-based validation.

[Mehak261124]

Hey @duburcqa, I have addressed your feedback and added the screenshot-based unit tests for draw_debug_frustum and draw_debug_trajectory. Could you take a look?