10
10
from revolve2 .simulation .simulator import RecordSettings
11
11
12
12
from ._control_interface_impl import ControlInterfaceImpl
13
- from ._custom_mujoco_viewer import CustomMujocoViewer
14
13
from ._open_gl_vision import OpenGLVision
15
14
from ._render_backend import RenderBackend
16
15
from ._scene_to_model import scene_to_model
17
16
from ._simulation_state_impl import SimulationStateImpl
17
+ from .viewers import CustomMujocoViewer , NativeMujocoViewer , ViewerType
18
18
19
19
20
20
def simulate_scene (
@@ -29,6 +29,7 @@ def simulate_scene(
29
29
simulation_timestep : float ,
30
30
cast_shadows : bool ,
31
31
fast_sim : bool ,
32
+ viewer_type : ViewerType ,
32
33
render_backend : RenderBackend = RenderBackend .EGL ,
33
34
) -> list [SimulationState ]:
34
35
"""
@@ -45,45 +46,73 @@ def simulate_scene(
45
46
:param simulation_timestep: The duration to integrate over during each step of the simulation. In seconds.
46
47
:param cast_shadows: If shadows are cast.
47
48
:param fast_sim: If fancy rendering is disabled.
49
+ :param viewer_type: The type of viewer used for the rendering in a window.
48
50
:param render_backend: The backend to be used for rendering (EGL by default and switches to GLFW if no cameras are on the robot).
49
51
:returns: The results of simulation. The number of returned states depends on `sample_step`.
52
+ :raises ValueError: If the viewer is not able to record.
50
53
"""
51
54
logging .info (f"Simulating scene { scene_id } " )
52
55
56
+ """Define mujoco data and model objects for simuating."""
53
57
model , mapping = scene_to_model (
54
58
scene , simulation_timestep , cast_shadows = cast_shadows , fast_sim = fast_sim
55
59
)
60
+ data = mujoco .MjData (model )
61
+
62
+ """Define a control interface for the mujoco simulation (used to control robots)."""
63
+ control_interface = ControlInterfaceImpl (
64
+ data = data , abstraction_to_mujoco_mapping = mapping
65
+ )
66
+ """Make separate viewer for camera sensors."""
67
+ camera_viewers = {
68
+ camera .camera_id : OpenGLVision (
69
+ model = model , camera = camera , headless = headless , open_gl_lib = render_backend
70
+ )
71
+ for camera in mapping .camera_sensor .values ()
72
+ }
73
+
74
+ """Define some additional control variables."""
75
+ last_control_time = 0.0
76
+ last_sample_time = 0.0
77
+ last_video_time = 0.0 # time at which last video frame was saved
78
+
79
+ simulation_states : list [SimulationState ] = (
80
+ []
81
+ ) # The measured states of the simulation
56
82
57
83
"""If we dont have cameras and the backend is not set we go to the default GLFW."""
58
84
if len (mapping .camera_sensor .values ()) == 0 :
59
85
render_backend = RenderBackend .GLFW
60
- data = mujoco .MjData (model )
61
86
87
+ """Initialize viewer object if we need to render the scene."""
62
88
if not headless or record_settings is not None :
63
- width , height = (
64
- (record_settings .width , record_settings .height )
65
- if record_settings is not None
66
- else (None , None )
67
- )
68
- viewer = CustomMujocoViewer (
89
+ match viewer_type :
90
+ case viewer_type .CUSTOM :
91
+ viewer = CustomMujocoViewer
92
+ case viewer_type .NATIVE :
93
+ viewer = NativeMujocoViewer
94
+ case _:
95
+ raise ValueError (
96
+ f"Viewer of type { viewer_type } not defined in _simulate_scene."
97
+ )
98
+
99
+ viewer = viewer (
69
100
model ,
70
101
data ,
71
- width = width ,
72
- height = height ,
102
+ width = None if record_settings is None else record_settings . width ,
103
+ height = None if record_settings is None else record_settings . height ,
73
104
backend = render_backend ,
74
105
start_paused = start_paused ,
75
106
render_every_frame = False ,
76
107
hide_menus = (record_settings is not None ),
77
108
)
78
109
79
- camera_viewers = {
80
- camera .camera_id : OpenGLVision (
81
- model = model , camera = camera , headless = headless , open_gl_lib = render_backend
82
- )
83
- for camera in mapping .camera_sensor .values ()
84
- }
85
-
110
+ """Record the scene if we want to record."""
86
111
if record_settings is not None :
112
+ if not viewer .can_record :
113
+ raise ValueError (
114
+ f"Selected Viewer { type (viewer ).__name__ } has no functionality to record."
115
+ )
87
116
video_step = 1 / record_settings .fps
88
117
video_file_path = f"{ record_settings .video_directory } /{ scene_id } .mp4"
89
118
fourcc = cv2 .VideoWriter .fourcc (* "mp4v" )
@@ -94,15 +123,10 @@ def simulate_scene(
94
123
viewer .current_viewport_size (),
95
124
)
96
125
97
- last_control_time = 0.0
98
- last_sample_time = 0.0
99
- last_video_time = 0.0 # time at which last video frame was saved
100
-
101
- # The measured states of the simulation
102
- simulation_states : list [SimulationState ] = []
103
-
104
- # Compute forward dynamics without actually stepping forward in time.
105
- # This updates the data so we can read out the initial state.
126
+ """
127
+ Compute forward dynamics without actually stepping forward in time.
128
+ This updates the data so we can read out the initial state.
129
+ """
106
130
mujoco .mj_forward (model , data )
107
131
images = {
108
132
camera_id : camera_viewer .process (model , data )
@@ -117,9 +141,7 @@ def simulate_scene(
117
141
)
118
142
)
119
143
120
- control_interface = ControlInterfaceImpl (
121
- data = data , abstraction_to_mujoco_mapping = mapping
122
- )
144
+ """After rendering the initial state, we enter the rendering loop."""
123
145
while (time := data .time ) < (
124
146
float ("inf" ) if simulation_time is None else simulation_time
125
147
):
@@ -147,6 +169,7 @@ def simulate_scene(
147
169
148
170
# step simulation
149
171
mujoco .mj_step (model , data )
172
+ # extract images from camera sensors.
150
173
images = {
151
174
camera_id : camera_viewer .process (model , data )
152
175
for camera_id , camera_viewer in camera_viewers .items ()
@@ -164,22 +187,23 @@ def simulate_scene(
164
187
165
188
# https://github.com/deepmind/mujoco/issues/285 (see also record.cc)
166
189
img : npt .NDArray [np .uint8 ] = np .empty (
167
- (viewer .viewport . height , viewer . viewport . width , 3 ),
190
+ (* viewer .current_viewport_size () , 3 ),
168
191
dtype = np .uint8 ,
169
192
)
170
193
171
194
mujoco .mjr_readPixels (
172
195
rgb = img ,
173
196
depth = None ,
174
- viewport = viewer .viewport ,
175
- con = viewer .ctx ,
197
+ viewport = viewer .view_port ,
198
+ con = viewer .context ,
176
199
)
177
200
# Flip the image and map to OpenCV colormap (BGR -> RGB)
178
201
img = np .flipud (img )[:, :, ::- 1 ]
179
202
video .write (img )
180
203
204
+ """Once simulation is done we close the potential viewer and release the potential video."""
181
205
if not headless or record_settings is not None :
182
- viewer .close ()
206
+ viewer .close_viewer ()
183
207
184
208
if record_settings is not None :
185
209
video .release ()
@@ -193,5 +217,4 @@ def simulate_scene(
193
217
)
194
218
195
219
logging .info (f"Scene { scene_id } done." )
196
-
197
220
return simulation_states
0 commit comments