Skip to content

Commit d746fa6

Browse files
committed
Native Viewer Support (#551)
1 parent 7ec5780 commit d746fa6

File tree

11 files changed

+303
-42
lines changed

11 files changed

+303
-42
lines changed

examples/1_simulator_basics/1a_simulate_single_robot/main.py

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,12 +73,15 @@ def main() -> None:
7373
"""
7474
After we have the scene ready we create a simulator that will perform the simulation.
7575
This tutorial chooses to use Mujoco, but your version of revolve might contain other simulators as well.
76+
77+
For mujoco we can select either the `native` mujoco viewer (more performance) or our `custom` viewer (which is more flexible for adjustments).
7678
"""
77-
simulator = LocalSimulator()
79+
simulator = LocalSimulator(viewer_type="native")
7880

7981
# `batch_parameters` are important parameters for simulation.
8082
# Here, we use the parameters that are standard in CI Group.
8183
batch_parameters = make_standard_batch_parameters()
84+
batch_parameters.simulation_time = 60 # Here we update our simulation time.
8285

8386
# Simulate the scene.
8487
# A simulator can run multiple sets of scenes sequentially; it can be reused.

examples/2_modular_robot_basics/2b_brain_with_feedback/main.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ def main() -> None:
136136
scene.add_robot(robot)
137137

138138
# Simulate the scene.
139-
simulator = LocalSimulator()
139+
simulator = LocalSimulator(viewer_type="native")
140140
simulate_scenes(
141141
simulator=simulator,
142142
batch_parameters=make_standard_batch_parameters(),

simulation/revolve2/simulation/simulator/__init__.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,5 +4,6 @@
44
from ._batch_parameters import BatchParameters
55
from ._record_settings import RecordSettings
66
from ._simulator import Simulator
7+
from ._viewer import Viewer
78

8-
__all__ = ["Batch", "BatchParameters", "RecordSettings", "Simulator"]
9+
__all__ = ["Batch", "BatchParameters", "RecordSettings", "Simulator", "Viewer"]
Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
from abc import ABC, abstractmethod
2+
from typing import Any
3+
4+
5+
class Viewer(ABC):
6+
"""An abstract viewer class, enabling the rendering of simulations."""
7+
8+
@abstractmethod
9+
def close_viewer(self) -> None:
10+
"""Close the viewer."""
11+
12+
@abstractmethod
13+
def render(self) -> Any | None:
14+
"""
15+
Render the scene on the viewer.
16+
17+
:returns: Nothing or feedback.
18+
"""
19+
20+
@abstractmethod
21+
def current_viewport_size(self) -> tuple[int, int]:
22+
"""
23+
Get the current viewport size.
24+
25+
:returns: The size as a tuple.
26+
"""
27+
28+
@property
29+
@abstractmethod
30+
def view_port(self) -> Any:
31+
"""
32+
Get the viewport.
33+
34+
:returns: The viewport object.
35+
"""
36+
37+
@property
38+
@abstractmethod
39+
def context(self) -> Any:
40+
"""
41+
Return the viewer context.
42+
43+
:returns: The context object.
44+
"""
45+
46+
@property
47+
@abstractmethod
48+
def can_record(self) -> bool:
49+
"""
50+
Check if this viewer can record.
51+
52+
:returns: The truth.
53+
"""

simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_local_simulator.py

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77

88
from ._simulate_manual_scene import simulate_manual_scene
99
from ._simulate_scene import simulate_scene
10+
from .viewers import ViewerType
1011

1112

1213
class LocalSimulator(Simulator):
@@ -18,6 +19,7 @@ class LocalSimulator(Simulator):
1819
_cast_shadows: bool
1920
_fast_sim: bool
2021
_manual_control: bool
22+
_viewer_type: ViewerType
2123

2224
def __init__(
2325
self,
@@ -27,6 +29,7 @@ def __init__(
2729
cast_shadows: bool = False,
2830
fast_sim: bool = False,
2931
manual_control: bool = False,
32+
viewer_type: ViewerType | str = ViewerType.CUSTOM,
3033
):
3134
"""
3235
Initialize this object.
@@ -37,6 +40,7 @@ def __init__(
3740
:param cast_shadows: Whether shadows are cast in the simulation.
3841
:param fast_sim: Whether more complex rendering prohibited.
3942
:param manual_control: Whether the simulation should be controlled manually.
43+
:param viewer_type: The viewer-implementation to use in the local simulator.
4044
"""
4145
assert (
4246
headless or num_simulators == 1
@@ -52,6 +56,11 @@ def __init__(
5256
self._cast_shadows = cast_shadows
5357
self._fast_sim = fast_sim
5458
self._manual_control = manual_control
59+
self._viewer_type = (
60+
ViewerType.from_string(viewer_type)
61+
if isinstance(viewer_type, str)
62+
else viewer_type
63+
)
5564

5665
def simulate_batch(self, batch: Batch) -> list[list[SimulationState]]:
5766
"""
@@ -101,6 +110,7 @@ def simulate_batch(self, batch: Batch) -> list[list[SimulationState]]:
101110
batch.parameters.simulation_timestep,
102111
self._cast_shadows,
103112
self._fast_sim,
113+
self._viewer_type,
104114
)
105115
for scene_index, scene in enumerate(batch.scenes)
106116
]
@@ -119,6 +129,7 @@ def simulate_batch(self, batch: Batch) -> list[list[SimulationState]]:
119129
batch.parameters.simulation_timestep,
120130
self._cast_shadows,
121131
self._fast_sim,
132+
self._viewer_type,
122133
)
123134
for scene_index, scene in enumerate(batch.scenes)
124135
]

simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_simulate_manual_scene.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,10 @@
1111
from revolve2.simulation.scene import Scene
1212

1313
from ._control_interface_impl import ControlInterfaceImpl
14-
from ._custom_mujoco_viewer import CustomMujocoViewer, CustomMujocoViewerMode
1514
from ._render_backend import RenderBackend
1615
from ._scene_to_model import scene_to_model
1716
from ._simulation_state_impl import SimulationStateImpl
17+
from .viewers import CustomMujocoViewer, CustomMujocoViewerMode
1818

1919

2020
def simulate_manual_scene(
@@ -94,5 +94,5 @@ def simulate_manual_scene(
9494
"""If we press ctrl-C this script will end with the finally clause."""
9595
pass
9696
finally:
97-
viewer.close()
97+
viewer.close_viewer()
9898
logging.info("Testing done.")

simulators/mujoco_simulator/revolve2/simulators/mujoco_simulator/_simulate_scene.py

Lines changed: 57 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -10,11 +10,11 @@
1010
from revolve2.simulation.simulator import RecordSettings
1111

1212
from ._control_interface_impl import ControlInterfaceImpl
13-
from ._custom_mujoco_viewer import CustomMujocoViewer
1413
from ._open_gl_vision import OpenGLVision
1514
from ._render_backend import RenderBackend
1615
from ._scene_to_model import scene_to_model
1716
from ._simulation_state_impl import SimulationStateImpl
17+
from .viewers import CustomMujocoViewer, NativeMujocoViewer, ViewerType
1818

1919

2020
def simulate_scene(
@@ -29,6 +29,7 @@ def simulate_scene(
2929
simulation_timestep: float,
3030
cast_shadows: bool,
3131
fast_sim: bool,
32+
viewer_type: ViewerType,
3233
render_backend: RenderBackend = RenderBackend.EGL,
3334
) -> list[SimulationState]:
3435
"""
@@ -45,45 +46,73 @@ def simulate_scene(
4546
:param simulation_timestep: The duration to integrate over during each step of the simulation. In seconds.
4647
:param cast_shadows: If shadows are cast.
4748
:param fast_sim: If fancy rendering is disabled.
49+
:param viewer_type: The type of viewer used for the rendering in a window.
4850
:param render_backend: The backend to be used for rendering (EGL by default and switches to GLFW if no cameras are on the robot).
4951
: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.
5053
"""
5154
logging.info(f"Simulating scene {scene_id}")
5255

56+
"""Define mujoco data and model objects for simuating."""
5357
model, mapping = scene_to_model(
5458
scene, simulation_timestep, cast_shadows=cast_shadows, fast_sim=fast_sim
5559
)
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
5682

5783
"""If we dont have cameras and the backend is not set we go to the default GLFW."""
5884
if len(mapping.camera_sensor.values()) == 0:
5985
render_backend = RenderBackend.GLFW
60-
data = mujoco.MjData(model)
6186

87+
"""Initialize viewer object if we need to render the scene."""
6288
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(
69100
model,
70101
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,
73104
backend=render_backend,
74105
start_paused=start_paused,
75106
render_every_frame=False,
76107
hide_menus=(record_settings is not None),
77108
)
78109

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."""
86111
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+
)
87116
video_step = 1 / record_settings.fps
88117
video_file_path = f"{record_settings.video_directory}/{scene_id}.mp4"
89118
fourcc = cv2.VideoWriter.fourcc(*"mp4v")
@@ -94,15 +123,10 @@ def simulate_scene(
94123
viewer.current_viewport_size(),
95124
)
96125

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+
"""
106130
mujoco.mj_forward(model, data)
107131
images = {
108132
camera_id: camera_viewer.process(model, data)
@@ -117,9 +141,7 @@ def simulate_scene(
117141
)
118142
)
119143

120-
control_interface = ControlInterfaceImpl(
121-
data=data, abstraction_to_mujoco_mapping=mapping
122-
)
144+
"""After rendering the initial state, we enter the rendering loop."""
123145
while (time := data.time) < (
124146
float("inf") if simulation_time is None else simulation_time
125147
):
@@ -147,6 +169,7 @@ def simulate_scene(
147169

148170
# step simulation
149171
mujoco.mj_step(model, data)
172+
# extract images from camera sensors.
150173
images = {
151174
camera_id: camera_viewer.process(model, data)
152175
for camera_id, camera_viewer in camera_viewers.items()
@@ -164,22 +187,23 @@ def simulate_scene(
164187

165188
# https://github.com/deepmind/mujoco/issues/285 (see also record.cc)
166189
img: npt.NDArray[np.uint8] = np.empty(
167-
(viewer.viewport.height, viewer.viewport.width, 3),
190+
(*viewer.current_viewport_size(), 3),
168191
dtype=np.uint8,
169192
)
170193

171194
mujoco.mjr_readPixels(
172195
rgb=img,
173196
depth=None,
174-
viewport=viewer.viewport,
175-
con=viewer.ctx,
197+
viewport=viewer.view_port,
198+
con=viewer.context,
176199
)
177200
# Flip the image and map to OpenCV colormap (BGR -> RGB)
178201
img = np.flipud(img)[:, :, ::-1]
179202
video.write(img)
180203

204+
"""Once simulation is done we close the potential viewer and release the potential video."""
181205
if not headless or record_settings is not None:
182-
viewer.close()
206+
viewer.close_viewer()
183207

184208
if record_settings is not None:
185209
video.release()
@@ -193,5 +217,4 @@ def simulate_scene(
193217
)
194218

195219
logging.info(f"Scene {scene_id} done.")
196-
197220
return simulation_states
Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
"""Different viewer implementations for mujoco."""
2+
3+
from ._custom_mujoco_viewer import CustomMujocoViewer, CustomMujocoViewerMode
4+
from ._native_mujoco_viewer import NativeMujocoViewer
5+
from ._viewer_type import ViewerType
6+
7+
__all__ = [
8+
"CustomMujocoViewer",
9+
"CustomMujocoViewerMode",
10+
"NativeMujocoViewer",
11+
"ViewerType",
12+
]

0 commit comments

Comments
 (0)