Simulating a Camera Sensor

Simulating a Camera Sensor#

This tutorial demonstrates how to create and simulate a camera sensor attached to a robot using SimulationManager. You will learn how to configure a camera, attach it to the robot’s end-effector, and visualize the sensor’s output during simulation.

Source Code#

The code for this tutorial is in scripts/tutorials/sim/create_sensor.py.

Show code for create_sensor.py
  1# ----------------------------------------------------------------------------
  2# Copyright (c) 2021-2025 DexForce Technology Co., Ltd.
  3#
  4# Licensed under the Apache License, Version 2.0 (the "License");
  5# you may not use this file except in compliance with the License.
  6# You may obtain a copy of the License at
  7#
  8#     http://www.apache.org/licenses/LICENSE-2.0
  9#
 10# Unless required by applicable law or agreed to in writing, software
 11# distributed under the License is distributed on an "AS IS" BASIS,
 12# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 13# See the License for the specific language governing permissions and
 14# limitations under the License.
 15# ----------------------------------------------------------------------------
 16
 17"""
 18This script demonstrates how to create and simulate a camera sensor attached to a robot using SimulationManager.
 19It shows how to configure a camera sensor, attach it to the robot's end-effector, and visualize the sensor's output during simulation.
 20"""
 21
 22import argparse
 23import cv2
 24import numpy as np
 25import time
 26import torch
 27
 28torch.set_printoptions(precision=4, sci_mode=False)
 29
 30from scipy.spatial.transform import Rotation as R
 31
 32from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
 33from embodichain.lab.sim.sensors import Camera, CameraCfg
 34from embodichain.lab.sim.objects import Robot
 35from embodichain.lab.sim.cfg import (
 36    JointDrivePropertiesCfg,
 37    RobotCfg,
 38    URDFCfg,
 39    RigidObjectCfg,
 40)
 41from embodichain.lab.sim.shapes import CubeCfg
 42from embodichain.data import get_data_path
 43
 44
 45def mask_to_color_map(mask, user_ids, fix_seed=True):
 46    """
 47    Convert instance mask into color map.
 48    :param mask: Instance mask map.
 49    :param user_ids: List of unique user IDs in the mask.
 50    :return: Color map.
 51    """
 52    # Create a blank RGB image
 53    color_map = np.zeros((mask.shape[0], mask.shape[1], 3), dtype=np.uint8)
 54
 55    # Generate deterministic colors based on user_id values
 56    colors = []
 57    for user_id in user_ids:
 58        # Use the user_id as seed to generate deterministic color
 59        np.random.seed(user_id)
 60        color = np.random.choice(range(256), size=3)
 61        colors.append(color)
 62
 63    for idx, color in enumerate(colors):
 64        # Assign color to the instances of each class
 65        color_map[mask == user_ids[idx]] = color
 66
 67    return color_map
 68
 69
 70def main():
 71    """Main function to demonstrate robot sensor simulation."""
 72
 73    # Parse command line arguments
 74    parser = argparse.ArgumentParser(
 75        description="Create and simulate a robot in SimulationManager"
 76    )
 77    parser.add_argument(
 78        "--num_envs", type=int, default=1, help="Number of environments to simulate"
 79    )
 80    parser.add_argument(
 81        "--device",
 82        type=str,
 83        default="cpu",
 84        choices=["cpu", "cuda"],
 85        help="Device to run simulation on",
 86    )
 87    parser.add_argument("--headless", action="store_true", help="Run in headless mode")
 88    parser.add_argument(
 89        "--enable_rt", action="store_true", help="Enable ray tracing rendering"
 90    )
 91    parser.add_argument(
 92        "--attach_sensor",
 93        action="store_true",
 94        help="Attach sensor to robot end-effector",
 95    )
 96    args = parser.parse_args()
 97
 98    # Initialize simulation
 99    print("Creating simulation...")
100    config = SimulationManagerCfg(
101        headless=True,
102        sim_device=args.device,
103        arena_space=3.0,
104        enable_rt=args.enable_rt,
105        physics_dt=1.0 / 100.0,
106    )
107    sim = SimulationManager(config)
108
109    # Build multiple environments if requested
110    if args.num_envs > 1:
111        sim.build_multiple_arenas(args.num_envs)
112
113    # Create robot configuration
114    robot = create_robot(sim)
115
116    sensor = create_sensor(sim, args)
117
118    # Add a cube to the scene
119    cube_cfg = RigidObjectCfg(
120        uid="cube",
121        shape=CubeCfg(size=[0.05, 0.05, 0.05]),  # Use CubeCfg for a cube
122        init_pos=[1.2, -0.2, 0.1],
123        init_rot=[0, 0, 0],
124    )
125    sim.add_rigid_object(cfg=cube_cfg)
126
127    # Initialize GPU physics if using CUDA
128    if sim.is_use_gpu_physics:
129        sim.init_gpu_physics()
130
131    # Open visualization window if not headless
132    if not args.headless:
133        sim.open_window()
134
135    # Run simulation loop
136    run_simulation(sim, robot, sensor)
137
138
139def create_sensor(sim: SimulationManager, args):
140    # intrinsics params
141    intrinsics = (600, 600, 320.0, 240.0)
142    width = 640
143    height = 480
144
145    # extrinsics params
146    pos = [0.09, 0.05, 0.04]
147    quat = R.from_euler("xyz", [-35, 135, 0], degrees=True).as_quat().tolist()
148
149    # If attach_sensor is True, attach to robot end-effector; otherwise, place it in the scene
150    if args.attach_sensor:
151        parent = "ee_link"
152    else:
153        parent = None
154        pos = [1.2, -0.2, 1.5]
155        quat = R.from_euler("xyz", [0, 180, 0], degrees=True).as_quat().tolist()
156        quat = [quat[3], quat[0], quat[1], quat[2]]  # Convert to (w, x, y, z)
157
158    # create camera sensor and attach to robot end-effector
159    camera: Camera = sim.add_sensor(
160        sensor_cfg=CameraCfg(
161            width=width,
162            height=height,
163            intrinsics=intrinsics,
164            extrinsics=CameraCfg.ExtrinsicsCfg(
165                parent=parent,
166                pos=pos,
167                quat=quat,
168            ),
169            near=0.01,
170            far=10.0,
171            enable_color=True,
172            enable_depth=True,
173            enable_mask=True,
174            enable_normal=True,
175        )
176    )
177    return camera
178
179
180def create_robot(sim):
181    """Create and configure a robot in the simulation."""
182
183    print("Loading robot...")
184
185    # Get SR5 URDF path
186    sr5_urdf_path = get_data_path("Rokae/SR5/SR5.urdf")
187
188    # Get hand URDF path
189    hand_urdf_path = get_data_path(
190        "BrainCoHandRevo1/BrainCoLeftHand/BrainCoLeftHand.urdf"
191    )
192
193    # Define control parts for the robot
194    # Joint names in control_parts can be regex patterns
195    CONTROL_PARTS = {
196        "arm": [
197            "JOINT[1-6]",  # Matches JOINT1, JOINT2, ..., JOINT6
198        ],
199        "hand": ["LEFT_.*"],  # Matches all joints starting with L_
200    }
201
202    # Define transformation for hand attachment
203    hand_attach_xpos = np.eye(4)
204    hand_attach_xpos[:3, :3] = R.from_rotvec([90, 0, 0], degrees=True).as_matrix()
205    hand_attach_xpos[2, 3] = 0.02
206
207    cfg = RobotCfg(
208        uid="sr5_with_brainco",
209        urdf_cfg=URDFCfg(
210            components=[
211                {
212                    "component_type": "arm",
213                    "urdf_path": sr5_urdf_path,
214                },
215                {
216                    "component_type": "hand",
217                    "urdf_path": hand_urdf_path,
218                    "transform": hand_attach_xpos,
219                },
220            ]
221        ),
222        control_parts=CONTROL_PARTS,
223        drive_pros=JointDrivePropertiesCfg(
224            stiffness={"JOINT[1-6]": 1e4, "LEFT_.*": 1e3},
225            damping={"JOINT[1-6]": 1e3, "LEFT_.*": 1e2},
226        ),
227    )
228
229    # Add robot to simulation
230    robot: Robot = sim.add_robot(cfg=cfg)
231
232    print(f"Robot created successfully with {robot.dof} joints")
233
234    return robot
235
236
237def get_sensor_image(camera: Camera, headless=False, step_count=0):
238    """
239    Get color, depth, mask, and normals views from the camera,
240    and visualize them in a 2x2 grid (or save if headless).
241    """
242    import matplotlib.pyplot as plt
243
244    camera.update()
245    data = camera.get_data()
246    # Get four views
247    rgba = data["color"].cpu().numpy()[0, :, :, :3]  # (H, W, 3)
248    depth = data["depth"].squeeze_().cpu().numpy()  # (H, W)
249    mask = data["mask"].squeeze_().cpu().numpy()  # (H, W)
250    normals = data["normal"].cpu().numpy()[0]  # (H, W, 3)
251
252    # Normalize for visualization
253    depth_vis = (depth - depth.min()) / (depth.ptp() + 1e-8)
254    depth_vis = (depth_vis * 255).astype("uint8")
255    mask_vis = mask_to_color_map(mask, user_ids=np.unique(mask))
256    normals_vis = ((normals + 1) / 2 * 255).astype("uint8")
257
258    # Prepare titles and images for display
259    titles = ["Color", "Depth", "Mask", "Normals"]
260    images = [
261        cv2.cvtColor(rgba, cv2.COLOR_RGB2BGR),
262        cv2.cvtColor(depth_vis, cv2.COLOR_GRAY2BGR),
263        mask_vis,
264        cv2.cvtColor(normals_vis, cv2.COLOR_RGB2BGR),
265    ]
266
267    if not headless:
268        # Concatenate images for 2x2 grid display using OpenCV
269        top = np.hstack([images[0], images[1]])
270        bottom = np.hstack([images[2], images[3]])
271        grid = np.vstack([top, bottom])
272        cv2.imshow("Sensor Views (Color / Depth / Mask / Normals)", grid)
273        cv2.waitKey(1)
274    else:
275        # Save the 2x2 grid as an image using matplotlib
276        fig, axs = plt.subplots(2, 2, figsize=(10, 8))
277        for ax, img, title in zip(axs.flatten(), images, titles):
278            ax.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
279            ax.set_title(title)
280            ax.axis("off")
281        plt.tight_layout()
282        plt.savefig(f"sensor_views_{step_count}.png")
283        plt.close(fig)
284
285
286def run_simulation(sim: SimulationManager, robot: Robot, camera: Camera):
287    """Run the simulation loop with robot and camera sensor control."""
288
289    print("Starting simulation...")
290    print("Robot will move through different poses")
291    print("Press Ctrl+C to stop")
292
293    step_count = 0
294
295    arm_joint_ids = robot.get_joint_ids("arm")
296    # Define some target joint positions for demonstration
297
298    arm_position1 = (
299        torch.tensor(
300            [0.0, 0.5, -1.5, 0.3, -0.5, 0], dtype=torch.float32, device=sim.device
301        )
302        .unsqueeze_(0)
303        .repeat(sim.num_envs, 1)
304    )
305
306    arm_position2 = (
307        torch.tensor(
308            [0.0, 0.5, -1.5, -0.3, -0.5, 0], dtype=torch.float32, device=sim.device
309        )
310        .unsqueeze_(0)
311        .repeat(sim.num_envs, 1)
312    )
313
314    try:
315        while True:
316            # Update physics
317            sim.update(step=1)
318
319            if step_count % 1001 == 0:
320                robot.set_qpos(qpos=arm_position1, joint_ids=arm_joint_ids)
321                print(f"Moving to arm position 1")
322
323                # Refresh and get image from sensor
324                get_sensor_image(camera)
325
326            if step_count % 2003 == 0:
327                robot.set_qpos(qpos=arm_position2, joint_ids=arm_joint_ids)
328                print(f"Moving to arm position 2")
329
330                # Refresh and get image from sensor
331                get_sensor_image(camera)
332
333            step_count += 1
334
335    except KeyboardInterrupt:
336        print("Stopping simulation...")
337    finally:
338        print("Cleaning up...")
339        sim.destroy()
340
341
342if __name__ == "__main__":
343    main()

Overview#

This tutorial builds on the basic robot simulation example. If you are not familiar with robot simulation in SimulationManager, please read the Simulating a Robot tutorial first.

1. Sensor Creation and Attachment#

The camera sensor is created using CameraCfg and can be attached to the robot’s end-effector or placed freely in the scene. The attachment is controlled by the --attach_sensor argument.

def create_sensor(sim: SimulationManager, args):
    # intrinsics params
    intrinsics = (600, 600, 320.0, 240.0)
    width = 640
    height = 480

    # extrinsics params
    pos = [0.09, 0.05, 0.04]
    quat = R.from_euler("xyz", [-35, 135, 0], degrees=True).as_quat().tolist()

    # If attach_sensor is True, attach to robot end-effector; otherwise, place it in the scene
    if args.attach_sensor:
        parent = "ee_link"
    else:
        parent = None
        pos = [1.2, -0.2, 1.5]
        quat = R.from_euler("xyz", [0, 180, 0], degrees=True).as_quat().tolist()
        quat = [quat[3], quat[0], quat[1], quat[2]]  # Convert to (w, x, y, z)

    # create camera sensor and attach to robot end-effector
    camera: Camera = sim.add_sensor(
        sensor_cfg=CameraCfg(
            width=width,
            height=height,
            intrinsics=intrinsics,
            extrinsics=CameraCfg.ExtrinsicsCfg(
                parent=parent,
                pos=pos,
                quat=quat,
            ),
            near=0.01,
            far=10.0,
            enable_color=True,
            enable_depth=True,
            enable_mask=True,
            enable_normal=True,
        )
    )
    return camera

  • The camera’s intrinsics (focal lengths and principal point) and resolution are set.

  • The extrinsics specify the camera’s pose relative to its parent (e.g., the robot’s ee_link or the world).

  • The camera is added to the simulation with sim.add_sensor().

2. Visualizing Sensor Output#

The function get_sensor_image retrieves and visualizes the camera’s color, depth, mask, and normal images. In GUI mode, images are shown in a 2x2 grid using OpenCV. In headless mode, images are saved to disk.

def get_sensor_image(camera: Camera, headless=False, step_count=0):
    """
    Get color, depth, mask, and normals views from the camera,
    and visualize them in a 2x2 grid (or save if headless).
    """
    import matplotlib.pyplot as plt

    camera.update()
    data = camera.get_data()
    # Get four views
    rgba = data["color"].cpu().numpy()[0, :, :, :3]  # (H, W, 3)
    depth = data["depth"].squeeze_().cpu().numpy()  # (H, W)
    mask = data["mask"].squeeze_().cpu().numpy()  # (H, W)
    normals = data["normal"].cpu().numpy()[0]  # (H, W, 3)

    # Normalize for visualization
    depth_vis = (depth - depth.min()) / (depth.ptp() + 1e-8)
    depth_vis = (depth_vis * 255).astype("uint8")
    mask_vis = mask_to_color_map(mask, user_ids=np.unique(mask))
    normals_vis = ((normals + 1) / 2 * 255).astype("uint8")

    # Prepare titles and images for display
    titles = ["Color", "Depth", "Mask", "Normals"]
    images = [
        cv2.cvtColor(rgba, cv2.COLOR_RGB2BGR),
        cv2.cvtColor(depth_vis, cv2.COLOR_GRAY2BGR),
        mask_vis,
        cv2.cvtColor(normals_vis, cv2.COLOR_RGB2BGR),
    ]

    if not headless:
        # Concatenate images for 2x2 grid display using OpenCV
        top = np.hstack([images[0], images[1]])
        bottom = np.hstack([images[2], images[3]])
        grid = np.vstack([top, bottom])
        cv2.imshow("Sensor Views (Color / Depth / Mask / Normals)", grid)
        cv2.waitKey(1)
    else:
        # Save the 2x2 grid as an image using matplotlib
        fig, axs = plt.subplots(2, 2, figsize=(10, 8))
        for ax, img, title in zip(axs.flatten(), images, titles):
            ax.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
            ax.set_title(title)
            ax.axis("off")
        plt.tight_layout()
        plt.savefig(f"sensor_views_{step_count}.png")
        plt.close(fig)

  • The camera is updated to capture the latest data.

  • Four types of images are visualized: color, depth, mask, and normals.

  • Images are displayed in a window or saved as PNG files depending on the mode.

3. Simulation Loop#

The simulation loop moves the robot through different arm poses and periodically updates and visualizes the sensor output.

def run_simulation(sim: SimulationManager, robot: Robot, camera: Camera):
    """Run the simulation loop with robot and camera sensor control."""

    print("Starting simulation...")
    print("Robot will move through different poses")
    print("Press Ctrl+C to stop")

    step_count = 0

    arm_joint_ids = robot.get_joint_ids("arm")
    # Define some target joint positions for demonstration

    arm_position1 = (
        torch.tensor(
            [0.0, 0.5, -1.5, 0.3, -0.5, 0], dtype=torch.float32, device=sim.device
        )
        .unsqueeze_(0)
        .repeat(sim.num_envs, 1)
    )

    arm_position2 = (
        torch.tensor(
            [0.0, 0.5, -1.5, -0.3, -0.5, 0], dtype=torch.float32, device=sim.device
        )
        .unsqueeze_(0)
        .repeat(sim.num_envs, 1)
    )

    try:
        while True:
            # Update physics
            sim.update(step=1)

            if step_count % 1001 == 0:
                robot.set_qpos(qpos=arm_position1, joint_ids=arm_joint_ids)
                print(f"Moving to arm position 1")

                # Refresh and get image from sensor
                get_sensor_image(camera)

            if step_count % 2003 == 0:
                robot.set_qpos(qpos=arm_position2, joint_ids=arm_joint_ids)
                print(f"Moving to arm position 2")

                # Refresh and get image from sensor
                get_sensor_image(camera)

            step_count += 1

    except KeyboardInterrupt:
        print("Stopping simulation...")
    finally:
        print("Cleaning up...")
        sim.destroy()

  • The robot alternates between two arm positions.

  • After each movement, the sensor image is refreshed and visualized.

Running the Example#

To run the sensor simulation script:

cd /home/dex/projects/yuanhaonan/embodichain
python scripts/tutorials/sim/create_sensor.py

You can customize the simulation with the following command-line options:

# Use GPU physics
python scripts/tutorials/sim/create_sensor.py --device cuda

# Simulate multiple environments
python scripts/tutorials/sim/create_sensor.py --num_envs 4

# Run in headless mode (no GUI, images saved to disk)
python scripts/tutorials/sim/create_sensor.py --headless

# Enable ray tracing rendering
python scripts/tutorials/sim/create_sensor.py --enable_rt

# Attach the camera to the robot end-effector
python scripts/tutorials/sim/create_sensor.py --attach_sensor

Key Features Demonstrated#

This tutorial demonstrates:

  1. Camera sensor creation using CameraCfg

  2. Sensor attachment to a robot link or placement in the scene

  3. Camera configuration (intrinsics, extrinsics, clipping planes)

  4. Real-time visualization of color, depth, mask, and normal images

  5. Robot-sensor integration in a simulation loop

Next Steps#

After completing this tutorial, you can explore:

  • Using other sensor types (e.g., stereo cameras, force sensors)

  • Recording sensor data for offline analysis

  • Integrating sensor feedback into robot control or learning algorithms

This tutorial provides a foundation for integrating perception into robotic simulation scenarios with SimulationManager. This tutorial provides the foundation for integrating perception into robotic simulation scenarios with SimulationManager.

Contents