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

            if cycle_step == 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 cycle_step == ACTION_SWITCH_INTERVAL:
                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 --renderer

# 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.