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