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