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

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

Contents