[FEATURE] Add RGBCamera sensor.

examples/sensors/camera_franka.py

@@ -0,0 +1,131 @@
+import argparse
+import os
+
+import numpy as np
+from tqdm import tqdm
+
+import genesis as gs
+from genesis.recorders.plotters import IS_MATPLOTLIB_AVAILABLE
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-dt", "--timestep", type=float, default=1e-2, help="Simulation time step")
+    parser.add_argument("-v", "--vis", action="store_true", help="Show visualization GUI", default=True)
+    parser.add_argument("-nv", "--no-vis", action="store_false", dest="vis", help="Disable visualization GUI")
+    parser.add_argument("-c", "--cpu", action="store_true", help="Use CPU instead of GPU")
+    parser.add_argument("-t", "--seconds", type=float, default=3, help="Number of seconds to simulate")
+    args = parser.parse_args()
+
+    ########################## init ##########################
+    gs.init(backend=gs.cpu if args.cpu else gs.gpu)
+
+    ########################## create a scene ##########################
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(
+            dt=args.timestep,
+        ),
+        vis_options=gs.options.VisOptions(
+            show_world_frame=False,
+        ),
+        viewer_options=gs.options.ViewerOptions(
+            camera_pos=(3.5, 0.0, 2.5),
+            camera_lookat=(0.0, 0.0, 0.5),
+            camera_fov=40,
+        ),
+        profiling_options=gs.options.ProfilingOptions(
+            show_FPS=False,
+        ),
+        # renderer=gs.renderers.BatchRenderer(),
+        show_viewer=args.vis,
+    )
+
+    ########################## entities ##########################
+    scene.add_entity(gs.morphs.Plane())
+    franka = scene.add_entity(
+        gs.morphs.MJCF(file="xml/franka_emika_panda/panda.xml"),
+    )
+    end_effector = franka.get_link("hand")
+    motors_dof = (0, 1, 2, 3, 4, 5, 6)
+
+    ########################## record sensor data ##########################
+    head_camera = scene.add_sensor(
+        gs.sensors.RGBCamera(
+            res=(360, 240),
+            pos=(2.0, 2.0, 1.0),
+            euler=(80.0, 0.0, 140.0),
+            # visualize
+            draw_debug=True,
+            debug_color=(0.0, 1.0, 1.0, 0.8),
+        )
+    )
+    hand_camera = scene.add_sensor(
+        gs.sensors.RGBCamera(
+            res=(128, 128),
+            entity_idx=franka.idx,
+            link_idx_local=end_effector.idx_local,
+            pos_offset=(0.0, 0.0, 0.15),
+            euler_offset=(180.0, 0.0, 0.0),
+            # visualize
+            draw_debug=True,
+        )
+    )
+    if args.vis:
+        if IS_MATPLOTLIB_AVAILABLE:
+            head_camera.start_recording(
+                gs.recorders.MPLImagePlot(
+                    title="Head Camera",
+                    # save_to_filename="camera_franka_head.mp4",
+                ),
+            )
+            hand_camera.start_recording(
+                gs.recorders.MPLImagePlot(
+                    title="Hand Camera",
+                    # save_to_filename="camera_franka_hand.mp4",
+                ),
+            )
+        else:
+            print("matplotlib not available, skipping real-time plotting.")
+
+    ########################## build ##########################
+    scene.build()
+
+    franka.set_dofs_kp(
+        np.array([4500.0, 4500.0, 3500.0, 3500.0, 2000.0, 2000.0, 2000.0, 100.0, 100.0]),
+    )
+    franka.set_dofs_kv(
+        np.array([450.0, 450.0, 350.0, 350.0, 200.0, 200.0, 200.0, 10.0, 10.0]),
+    )
+    franka.set_dofs_force_range(
+        np.array([-87.0, -87.0, -87.0, -87.0, -12.0, -12.0, -12.0, -100.0, -100.0]),
+        np.array([87.0, 87.0, 87.0, 87.0, 12.0, 12.0, 12.0, 100.0, 100.0]),
+    )
+
+    circle_center = np.array([0.4, 0.0, 0.5])
+    circle_radius = 0.15
+    rate = np.deg2rad(2.0)
+
+    try:
+        steps = int(args.seconds / args.timestep) if "PYTEST_VERSION" not in os.environ else 5
+        for i in tqdm(range(steps)):
+            scene.step()
+
+            # control franka to move in a circle and draw the target positions
+            pos = circle_center + np.array([np.cos(i * rate), np.sin(i * rate), 0]) * circle_radius
+            qpos = franka.inverse_kinematics(
+                link=end_effector,
+                pos=pos,
+                quat=np.array([0.0, 1.0, 0.0, 0.0]),
+            )
+            franka.control_dofs_position(qpos[:-2], motors_dof)
+
+    except KeyboardInterrupt:
+        gs.logger.info("Simulation interrupted, exiting.")
+    finally:
+        gs.logger.info("Simulation finished.")
+
+        scene.stop_recording()
+
+
+if __name__ == "__main__":
+    main()

genesis/engine/scene.py

@@ -633,6 +633,9 @@ def add_camera(
 
         Warning
         -------
+        This method is deprecated and may change or be removed in a future version. This returns a gs.Camera instead of
+        a gs.sensors.Camera. Please use the sensors API with `Scene.add_sensor(gs.sensors.Camera(...))` instead.
+
         When 'pinhole' is used, the `aperture` and `focal_len` parameters are ignored.
 
         Parameters
@@ -685,6 +688,11 @@ def add_camera(
         camera : genesis.Camera
             The created camera object.
         """
+        gs.logger.warning(
+            "`Scene.add_camera` is deprecated and will be removed in a future version."
+            " Please use `Scene.add_sensor(gs.sensors.Camera(...))` instead."
+        )
+
         if denoise is None:
             denoise = sys.platform != "darwin"
         return self._visualizer.add_camera(
@@ -805,19 +813,20 @@ def build(
             with open(os.devnull, "w") as stderr, redirect_libc_stderr(stderr):
                 self._sim.build()
 
-            # reset state
-            self._reset()
-
             self._is_built = True
-
-        with gs.logger.timer("Compiling simulation kernels..."):
-            self._sim.step()
-            self._reset()
+            self._init_state = self._get_state()
 
         # visualizer
         with gs.logger.timer("Building visualizer..."):
             self._visualizer.build()
 
+        # reset state
+        self._reset()
+
+        with gs.logger.timer("Compiling simulation kernels..."):
+            self._sim.step()
+            self._reset()
+
         if self.profiling_options.show_FPS:
             self.FPS_tracker = FPSTracker(self.n_envs, alpha=self.profiling_options.FPS_tracker_alpha)
 
@@ -891,15 +900,12 @@ def reset(self, state: SimState | None = None, envs_idx=None):
         self._recorder_manager.reset(envs_idx)
 
     def _reset(self, state: SimState | None = None, *, envs_idx=None):
-        if self._is_built:
-            if state is None:
-                state = self._init_state
-            else:
-                assert isinstance(state, SimState), "state must be a SimState object"
-                self._init_state = state
-            self._sim.reset(state, envs_idx)
+        if state is None:
+            state = self._init_state
         else:
-            self._init_state = self._get_state()
+            assert isinstance(state, SimState), "state must be a SimState object"
+            self._init_state = state
+        self._sim.reset(state, envs_idx)
 
         self._t = 0
         self._forward_ready = True

genesis/sensors/__init__.py

@@ -5,4 +5,5 @@
 from .raycaster import DepthCameraOptions as DepthCamera
 from .raycaster import RaycasterOptions as Lidar
 from .raycaster import RaycasterOptions as Raycaster
+from .rgb_camera import RGBCameraOptions as RGBCamera
 from .sensor_manager import SensorManager

genesis/sensors/base_sensor.py

@@ -357,6 +357,9 @@ class RigidSensorOptionsMixin:
 
     def validate(self, scene: "Scene"):
         super().validate(scene)
+        self.validate_rigid_link(scene)
+
+    def validate_rigid_link(self, scene: "Scene"):
         if self.entity_idx < 0 or self.entity_idx >= len(scene.entities):
             gs.raise_exception(f"Invalid RigidEntity index {self.entity_idx}.")
         entity = scene.entities[self.entity_idx]

genesis/sensors/contact_force.py

@@ -7,7 +7,7 @@
 
 import genesis as gs
 from genesis.engine.solvers import RigidSolver
-from genesis.utils.geom import ti_inv_transform_by_quat, trans_to_T, transform_by_quat
+from genesis.utils.geom import ti_inv_transform_by_quat, transform_by_quat
 from genesis.utils.misc import concat_with_tensor, make_tensor_field, tensor_to_array
 
 from .base_sensor import (
@@ -21,7 +21,6 @@
     Sensor,
     SensorOptions,
     SharedSensorMetadata,
-    _to_tuple,
 )
 from .sensor_manager import register_sensor