17
17
import os
18
18
from typing import List , Optional , Tuple , Union
19
19
20
- import mujoco
21
- from mujoco import _structs , _enums , _functions
22
-
23
- import mujoco .usd .camera as camera_module
24
- import mujoco .usd .lights as light_module
25
- import mujoco .usd .objects as object_module
26
- import mujoco .usd .shapes as shapes_module
27
- import mujoco .usd .utils as utils_module
28
20
import numpy as np
29
21
from PIL import Image as im
30
22
from PIL import ImageOps
31
-
32
23
# TODO: b/288149332 - Remove once USD Python Binding works well with pytype.
33
24
# pytype: disable=module-attr
34
25
from pxr import Sdf
35
26
from pxr import Usd
36
27
from pxr import UsdGeom
37
28
29
+ import mujoco
30
+ from mujoco import _enums
31
+ from mujoco import _functions
32
+ from mujoco import _structs
33
+ import mujoco .usd .camera as camera_module
34
+ import mujoco .usd .lights as light_module
35
+ import mujoco .usd .objects as object_module
36
+ import mujoco .usd .shapes as shapes_module
37
+
38
38
PRIMARY_CAMERA_NAME = "primary_camera"
39
39
40
+
40
41
class USDExporter :
41
42
"""MuJoCo to USD exporter for porting scenes to external renderers."""
42
43
@@ -139,48 +140,6 @@ def _initialize_output_directories(self):
139
140
if self .verbose :
140
141
print (f"Writing output frames and assets to { self .output_directory_path } " )
141
142
142
- def _update_scene (
143
- self ,
144
- data : mujoco .MjData ,
145
- camera : Union [int , str ] = - 1 ,
146
- scene_option : Optional [mujoco .MjvOption ] = None ,
147
- ):
148
- """Updates the scene."""
149
- camera_id = camera
150
- if isinstance (camera_id , str ):
151
- camera_id = mujoco .mj_name2id (
152
- self .model , mujoco .mjtObj .mjOBJ_CAMERA .value , camera_id
153
- )
154
- if camera_id == - 1 :
155
- raise ValueError (f"The camera '{ camera } ' does not exist." )
156
- if camera_id < - 1 or camera_id >= self .model .ncam :
157
- raise ValueError (
158
- f"The camera id { camera_id } is out of range [-1, { self .model .ncam } )."
159
- )
160
-
161
- # Render camera.
162
- camera = mujoco .MjvCamera ()
163
- camera .fixedcamid = camera_id
164
-
165
- # Defaults to mjCAMERA_FREE, otherwise mjCAMERA_FIXED refers to a
166
- # camera explicitly defined in the model.
167
- if camera_id == - 1 :
168
- camera .type = mujoco .mjtCamera .mjCAMERA_FREE
169
- mujoco .mjv_defaultFreeCamera (self .model , camera )
170
- else :
171
- camera .type = mujoco .mjtCamera .mjCAMERA_FIXED
172
-
173
- scene_option = scene_option or self ._scene_option
174
- mujoco .mjv_updateScene (
175
- self .model ,
176
- data ,
177
- scene_option ,
178
- None ,
179
- camera ,
180
- mujoco .mjtCatBit .mjCAT_ALL .value ,
181
- self ._scene ,
182
- )
183
-
184
143
def _update_scene (
185
144
self ,
186
145
data : mujoco .MjData ,
@@ -189,7 +148,7 @@ def _update_scene(
189
148
) -> None :
190
149
if not isinstance (camera , _structs .MjvCamera ):
191
150
if self .model .ncam == 0 :
192
- raise ValueError (f' No fixed cameras defined in mujoco model.' )
151
+ raise ValueError (f" No fixed cameras defined in mujoco model." )
193
152
camera_id = camera
194
153
if isinstance (camera_id , str ):
195
154
camera_id = _functions .mj_name2id (
@@ -198,10 +157,12 @@ def _update_scene(
198
157
if camera_id == - 1 :
199
158
raise ValueError (f'The camera "{ camera } " does not exist.' )
200
159
if camera_id == - 1 :
201
- raise ValueError (' Free cameras are not supported during USD export.' )
160
+ raise ValueError (" Free cameras are not supported during USD export." )
202
161
if camera_id < 0 or camera_id >= self .model .ncam :
203
- raise ValueError (f'The camera id { camera_id } is out of'
204
- f' range [-1, { self .model .ncam } ).' )
162
+ raise ValueError (
163
+ f"The camera id { camera_id } is out of"
164
+ f" range [-1, { self .model .ncam } )."
165
+ )
205
166
206
167
assert camera_id != - 1
207
168
@@ -216,7 +177,8 @@ def _update_scene(
216
177
data ,
217
178
scene_option ,
218
179
None ,
219
- camera , _enums .mjtCatBit .mjCAT_ALL .value ,
180
+ camera ,
181
+ _enums .mjtCatBit .mjCAT_ALL .value ,
220
182
self ._scene ,
221
183
)
222
184
@@ -274,9 +236,7 @@ def _load_textures(self):
274
236
relative_path = os .path .relpath (
275
237
self .assets_directory , self .frames_directory
276
238
)
277
- img_path = os .path .join (
278
- relative_path , texture_file_name
279
- )
239
+ img_path = os .path .join (relative_path , texture_file_name )
280
240
281
241
self .texture_files .append (img_path )
282
242
@@ -297,7 +257,9 @@ def _load_geom(self, geom: mujoco.MjvGeom):
297
257
geom_textures = []
298
258
else :
299
259
geom_textures = [
300
- (self .texture_files [i .item ()], self .model .tex_type [i .item ()]) if i .item () != - 1 else None
260
+ (self .texture_files [i .item ()], self .model .tex_type [i .item ()])
261
+ if i .item () != - 1
262
+ else None
301
263
for i in self .model .mat_texid [geom .matid ]
302
264
]
303
265
@@ -319,7 +281,7 @@ def _load_geom(self, geom: mujoco.MjvGeom):
319
281
name = geom_name ,
320
282
geom_type = geom .type ,
321
283
size = np .array ([1.0 , 1.0 , 1.0 ]),
322
- decouple = True
284
+ decouple = True ,
323
285
)
324
286
usd_geom = object_module .USDTendon (
325
287
mesh_config = mesh_config ,
@@ -333,9 +295,7 @@ def _load_geom(self, geom: mujoco.MjvGeom):
333
295
# handling primitives in our scene
334
296
else :
335
297
mesh_config = shapes_module .mesh_config_generator (
336
- name = geom_name ,
337
- geom_type = geom .type ,
338
- size = geom .size
298
+ name = geom_name , geom_type = geom .type , size = geom .size
339
299
)
340
300
usd_geom = object_module .USDPrimitiveMesh (
341
301
mesh_config = mesh_config ,
@@ -408,15 +368,21 @@ def _update_lights(self) -> None:
408
368
409
369
def _load_cameras (self ) -> None :
410
370
self .usd_cameras = {}
411
-
371
+
412
372
# add a primary camera for which the scene will be rendered
413
- self .usd_cameras [PRIMARY_CAMERA_NAME ] = camera_module .USDCamera (stage = self .stage , camera_name = PRIMARY_CAMERA_NAME )
373
+ self .usd_cameras [PRIMARY_CAMERA_NAME ] = camera_module .USDCamera (
374
+ stage = self .stage , camera_name = PRIMARY_CAMERA_NAME
375
+ )
414
376
if self .camera_names is not None :
415
377
for camera_name in self .camera_names :
416
- self .usd_cameras [camera_name ] = camera_module .USDCamera (stage = self .stage , camera_name = camera_name )
378
+ self .usd_cameras [camera_name ] = camera_module .USDCamera (
379
+ stage = self .stage , camera_name = camera_name
380
+ )
417
381
418
382
def _get_camera_orientation (self ) -> Tuple [_structs .MjvGLCamera , np .ndarray ]:
419
- avg_camera = mujoco .mjv_averageCamera (self ._scene .camera [0 ], self ._scene .camera [1 ])
383
+ avg_camera = mujoco .mjv_averageCamera (
384
+ self ._scene .camera [0 ], self ._scene .camera [1 ]
385
+ )
420
386
421
387
forward = avg_camera .forward
422
388
up = avg_camera .up
@@ -437,19 +403,23 @@ def _update_cameras(
437
403
) -> None :
438
404
# first, update the primary camera given the new scene
439
405
self ._update_scene (
440
- data , camera = camera , scene_option = scene_option ,
406
+ data ,
407
+ camera = camera ,
408
+ scene_option = scene_option ,
441
409
)
442
410
avg_camera , R = self ._get_camera_orientation ()
443
- self .usd_cameras [PRIMARY_CAMERA_NAME ].update (cam_pos = avg_camera .pos , cam_mat = R , frame = self .updates )
411
+ self .usd_cameras [PRIMARY_CAMERA_NAME ].update (
412
+ cam_pos = avg_camera .pos , cam_mat = R , frame = self .updates
413
+ )
444
414
445
415
# update the names of the fixed cameras in the scene
446
416
if self .camera_names is not None :
447
417
for camera_name in self .camera_names :
448
- self ._update_scene (
449
- data , camera = camera_name , scene_option = scene_option
450
- )
418
+ self ._update_scene (data , camera = camera_name , scene_option = scene_option )
451
419
avg_camera , R = self ._get_camera_orientation ()
452
- self .usd_cameras [camera_name ].update (cam_pos = avg_camera .pos , cam_mat = R , frame = self .updates )
420
+ self .usd_cameras [camera_name ].update (
421
+ cam_pos = avg_camera .pos , cam_mat = R , frame = self .updates
422
+ )
453
423
454
424
def add_light (
455
425
self ,
@@ -479,7 +449,9 @@ def add_light(
479
449
pos = np .array (pos ), intensity = intensity , color = color , frame = 0
480
450
)
481
451
elif light_type == "dome" :
482
- new_light = light_module .USDDomeLight (stage = self .stage , light_name = light_name )
452
+ new_light = light_module .USDDomeLight (
453
+ stage = self .stage , light_name = light_name
454
+ )
483
455
new_light .update (intensity = intensity , color = color )
484
456
485
457
def add_camera (
@@ -496,7 +468,8 @@ def add_camera(
496
468
camera_name: name of the camera to be stored in USD
497
469
"""
498
470
new_camera = camera_module .USDCamera (
499
- stage = self .stage , camera_name = camera_name )
471
+ stage = self .stage , camera_name = camera_name
472
+ )
500
473
501
474
rotation = np .zeros (9 )
502
475
quat = np .zeros (4 )
@@ -507,18 +480,18 @@ def add_camera(
507
480
def save_scene (self , filetype : str = "usd" ) -> None :
508
481
"""Saves the current scene as a USD trajectory
509
482
Args:
510
- filetype: type of USD file to save the scene as (options include
483
+ filetype: type of USD file to save the scene as (options include
511
484
"usd", "usda", and "usdc")
512
485
"""
513
486
self .stage .SetEndTimeCode (self .frame_count )
514
487
515
488
# post-processing for visibility of geoms in scene
516
489
for _ , geom_ref in self .geom_refs .items ():
517
- geom_ref .update_visibility (False , geom_ref .last_visible_frame + 1 )
490
+ geom_ref .update_visibility (False , geom_ref .last_visible_frame + 1 )
518
491
519
492
self .stage .Export (
520
- f"{ self .output_directory_root } /{ self .output_directory } /" +
521
- f"frames/frame_{ self .frame_count } .{ filetype } "
493
+ f"{ self .output_directory_root } /{ self .output_directory } /"
494
+ + f"frames/frame_{ self .frame_count } .{ filetype } "
522
495
)
523
496
if self .verbose :
524
497
print (f"Completed writing frame_{ self .frame_count } .{ filetype } " )
0 commit comments