1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305 | # Copyright (c) 2022-2024, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script demonstrates the different camera sensors that can be attached to a robot.
.. code-block:: bash
# Usage
./isaaclab.sh -p source/standalone/demos/cameras.py --disable_fabric
# Usage in headless mode
./isaaclab.sh -p source/standalone/demos/cameras.py --headless --enable_cameras --disable_fabric
"""
"""Launch Isaac Sim Simulator first."""
import argparse
from omni.isaac.lab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="This script demonstrates the different camera sensor implementations.")
parser.add_argument("--num_envs", type=int, default=4, help="Number of environments to spawn.")
parser.add_argument("--disable_fabric", action="store_true", help="Disable Fabric API and use USD instead.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
"""Rest everything follows."""
import matplotlib.pyplot as plt
import numpy as np
import os
import torch
import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg
from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg
from omni.isaac.lab.sensors import CameraCfg, RayCasterCameraCfg, TiledCameraCfg
from omni.isaac.lab.sensors.ray_caster import patterns
from omni.isaac.lab.terrains import TerrainImporterCfg
from omni.isaac.lab.utils import configclass
##
# Pre-defined configs
##
from omni.isaac.lab.terrains.config.rough import ROUGH_TERRAINS_CFG # isort:skip
from omni.isaac.lab_assets.anymal import ANYMAL_C_CFG # isort: skip
@configclass
class SensorsSceneCfg(InteractiveSceneCfg):
"""Design the scene with sensors on the robot."""
# ground plane
ground = TerrainImporterCfg(
prim_path="/World/ground",
max_init_terrain_level=None,
terrain_type="generator",
terrain_generator=ROUGH_TERRAINS_CFG.replace(color_scheme="random"),
visual_material=None,
debug_vis=False,
)
# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75))
)
# robot
robot: ArticulationCfg = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# sensors
camera = CameraCfg(
prim_path="{ENV_REGEX_NS}/Robot/base/front_cam",
update_period=0.1,
height=480,
width=640,
data_types=["rgb", "distance_to_image_plane"],
spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5)
),
offset=CameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"),
)
tiled_camera = TiledCameraCfg(
prim_path="{ENV_REGEX_NS}/Robot/base/front_cam",
update_period=0.1,
height=480,
width=640,
data_types=["rgb", "distance_to_camera"],
spawn=None, # the camera is already spawned in the scene
offset=TiledCameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"),
)
raycast_camera = RayCasterCameraCfg(
prim_path="{ENV_REGEX_NS}/Robot/base",
mesh_prim_paths=["/World/ground"],
update_period=0.1,
offset=RayCasterCameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"),
data_types=["distance_to_image_plane", "normals"],
pattern_cfg=patterns.PinholeCameraPatternCfg(
focal_length=24.0,
horizontal_aperture=20.955,
height=480,
width=640,
),
)
def save_images_grid(
images: list[torch.Tensor],
cmap: str | None = None,
nrow: int = 1,
subtitles: list[str] | None = None,
title: str | None = None,
filename: str | None = None,
):
"""Save images in a grid with optional subtitles and title.
Args:
images: A list of images to be plotted. Shape of each image should be (H, W, C).
cmap: Colormap to be used for plotting. Defaults to None, in which case the default colormap is used.
nrows: Number of rows in the grid. Defaults to 1.
subtitles: A list of subtitles for each image. Defaults to None, in which case no subtitles are shown.
title: Title of the grid. Defaults to None, in which case no title is shown.
filename: Path to save the figure. Defaults to None, in which case the figure is not saved.
"""
# show images in a grid
n_images = len(images)
ncol = int(np.ceil(n_images / nrow))
fig, axes = plt.subplots(nrow, ncol, figsize=(ncol * 2, nrow * 2))
axes = axes.flatten()
# plot images
for idx, (img, ax) in enumerate(zip(images, axes)):
img = img.detach().cpu().numpy()
ax.imshow(img, cmap=cmap)
ax.axis("off")
if subtitles:
ax.set_title(subtitles[idx])
# remove extra axes if any
for ax in axes[n_images:]:
fig.delaxes(ax)
# set title
if title:
plt.suptitle(title)
# adjust layout to fit the title
plt.tight_layout()
# save the figure
if filename:
os.makedirs(os.path.dirname(filename), exist_ok=True)
plt.savefig(filename)
# close the figure
plt.close()
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
"""Run the simulator."""
# Define simulation stepping
sim_dt = sim.get_physics_dt()
sim_time = 0.0
count = 0
# Create output directory to save images
output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "output")
os.makedirs(output_dir, exist_ok=True)
# Simulate physics
while simulation_app.is_running():
# Reset
if count % 500 == 0:
# reset counter
count = 0
# reset the scene entities
# root state
# we offset the root state by the origin since the states are written in simulation world frame
# if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world
root_state = scene["robot"].data.default_root_state.clone()
root_state[:, :3] += scene.env_origins
scene["robot"].write_root_state_to_sim(root_state)
# set joint positions with some noise
joint_pos, joint_vel = (
scene["robot"].data.default_joint_pos.clone(),
scene["robot"].data.default_joint_vel.clone(),
)
joint_pos += torch.rand_like(joint_pos) * 0.1
scene["robot"].write_joint_state_to_sim(joint_pos, joint_vel)
# clear internal buffers
scene.reset()
print("[INFO]: Resetting robot state...")
# Apply default actions to the robot
# -- generate actions/commands
targets = scene["robot"].data.default_joint_pos
# -- apply action to the robot
scene["robot"].set_joint_position_target(targets)
# -- write data to sim
scene.write_data_to_sim()
# perform step
sim.step()
# update sim-time
sim_time += sim_dt
count += 1
# update buffers
scene.update(sim_dt)
# print information from the sensors
print("-------------------------------")
print(scene["camera"])
print("Received shape of rgb image: ", scene["camera"].data.output["rgb"].shape)
print("Received shape of depth image: ", scene["camera"].data.output["distance_to_image_plane"].shape)
print("-------------------------------")
print(scene["tiled_camera"])
print("Received shape of rgb image: ", scene["tiled_camera"].data.output["rgb"].shape)
print("Received shape of depth image: ", scene["tiled_camera"].data.output["distance_to_camera"].shape)
print("-------------------------------")
print(scene["raycast_camera"])
print("Received shape of depth: ", scene["raycast_camera"].data.output["distance_to_image_plane"].shape)
print("Received shape of normals: ", scene["raycast_camera"].data.output["normals"].shape)
# save every 10th image (for visualization purposes only)
# note: saving images will slow down the simulation
if count % 10 == 0:
# compare generated RGB images across different cameras
rgb_images = [scene["camera"].data.output["rgb"][0, ..., :3], scene["tiled_camera"].data.output["rgb"][0]]
save_images_grid(
rgb_images,
subtitles=["Camera", "TiledCamera"],
title="RGB Image: Cam0",
filename=os.path.join(output_dir, "rgb", f"{count:04d}.jpg"),
)
# compare generated Depth images across different cameras
depth_images = [
scene["camera"].data.output["distance_to_image_plane"][0],
scene["tiled_camera"].data.output["distance_to_camera"][0, ..., 0],
scene["raycast_camera"].data.output["distance_to_image_plane"][0],
]
save_images_grid(
depth_images,
cmap="turbo",
subtitles=["Camera", "TiledCamera", "RaycasterCamera"],
title="Depth Image: Cam0",
filename=os.path.join(output_dir, "distance_to_camera", f"{count:04d}.jpg"),
)
# save all tiled RGB images
tiled_images = scene["tiled_camera"].data.output["rgb"]
save_images_grid(
tiled_images,
subtitles=[f"Cam{i}" for i in range(tiled_images.shape[0])],
title="Tiled RGB Image",
filename=os.path.join(output_dir, "tiled_rgb", f"{count:04d}.jpg"),
)
# save all camera RGB images
cam_images = scene["camera"].data.output["rgb"][..., :3]
save_images_grid(
cam_images,
subtitles=[f"Cam{i}" for i in range(cam_images.shape[0])],
title="Camera RGB Image",
filename=os.path.join(output_dir, "cam_rgb", f"{count:04d}.jpg"),
)
def main():
"""Main function."""
# note: tile rendered cameras doesn't update the camera poses when using the GPU pipeline and Fabric.
# this is a bug which should be fixed in the future releases.
sim_cfg = sim_utils.SimulationCfg(dt=0.005)
# check if fabric is enabled
if args_cli.disable_fabric:
sim_cfg.use_fabric = False
sim_cfg.device = "cpu"
# Initialize the simulation context
sim = sim_utils.SimulationContext(sim_cfg)
# Set main camera
sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0])
# design scene
scene_cfg = SensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
scene = InteractiveScene(scene_cfg)
# Play the simulator
sim.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# Run the simulator
run_simulator(sim, scene)
if __name__ == "__main__":
# run the main function
main()
# close sim app
simulation_app.close()
|
