From 21039fe69774477fa6a4ff256a06b86a51ca5457 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Thu, 12 Jun 2025 18:55:53 +0200 Subject: [PATCH 1/9] Refactor rendering code First we now use EGL for rendering enabling headless for HPC. We do not link against glfw nor egl anymore. Instead we find and load the required libraries dynamically on the python side (cf. egl_bootstrap.py). There is only one rendering context, created in python. We just pass the function to make it current to the C++ side and the C++ extension can just call that. Finally remove the custom renderer: We keep the shared memory approach, just remove the gui implement in C++. Instead we can use mujoco.viewer.launch_passive in a new process to get the full simulate GUI (without interaction). --- CMakeLists.txt | 24 -------- python/rcs/egl_bootstrap.py | 29 +++++++++ python/rcs/sim.py | 19 ++++-- src/common/CMakeLists.txt | 2 +- src/common/utils.cpp | 25 ++++++++ src/common/utils.h | 3 + src/pybind/rcs.cpp | 14 ++++- src/sim/CMakeLists.txt | 6 +- src/sim/gui.h | 15 ++++- src/sim/gui_client.cpp | 113 ++++++------------------------------ src/sim/renderer.cpp | 28 +++------ src/sim/sim.h | 3 +- 12 files changed, 128 insertions(+), 153 deletions(-) create mode 100644 python/rcs/egl_bootstrap.py create mode 100644 src/common/utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 19398e8f..7663218b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,6 @@ option(INCLUDE_UTN_MODELS "Whether to include the private UTN models. Requires G include(FetchContent) find_package(Eigen3 REQUIRED) -find_package(glfw3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(MuJoCo REQUIRED) @@ -69,29 +68,6 @@ FetchContent_Declare(pybind11 GIT_PROGRESS TRUE EXCLUDE_FROM_ALL ) -set(MuJoCo_UI_URL https://raw.githubusercontent.com/google-deepmind/mujoco/${MuJoCo_VERSION}/simulate/) -set(MuJoCo_UI_FILES platform_ui_adapter.cc platform_ui_adapter.h glfw_adapter.cc glfw_adapter.h glfw_adapter.cc glfw_corevideo.h glfw_corevideo.mm glfw_dispatch.cc glfw_dispatch.h) -# Set the destination directory for the downloaded files -set(MuJoCo_UI_DIR "${CMAKE_BINARY_DIR}/mujoco_ui") - -# Create the directory if it doesn't exist -file(MAKE_DIRECTORY ${MuJoCo_UI_DIR}) - -# Loop through each file and download it only if it doesn't already exist -foreach(file ${MuJoCo_UI_FILES}) - set(output_file "${MuJoCo_UI_DIR}/${file}") - if (NOT EXISTS ${output_file}) - message(STATUS "Downloading ${file}...") - file(DOWNLOAD ${MuJoCo_UI_URL}${file} ${output_file}) - else() - message(STATUS "File ${file} already exists, skipping download.") - endif() -endforeach() - -# Add the include directories or link the files as needed -add_library(mujoco_ui_lib ${MuJoCo_UI_DIR}/glfw_adapter.cc ${MuJoCo_UI_DIR}/platform_ui_adapter.cc ${MuJoCo_UI_DIR}/glfw_dispatch.cc) -target_link_libraries(mujoco_ui_lib MuJoCo::MuJoCo glfw) -target_include_directories(mujoco_ui_lib INTERFACE ${MuJoCo_UI_DIR}) FetchContent_MakeAvailable(libfranka rl pybind11) diff --git a/python/rcs/egl_bootstrap.py b/python/rcs/egl_bootstrap.py new file mode 100644 index 00000000..64090c76 --- /dev/null +++ b/python/rcs/egl_bootstrap.py @@ -0,0 +1,29 @@ +import ctypes +import ctypes.util +import os +import platform + +import mujoco.egl +import rcs._core as _cxx +from mujoco.egl import GLContext +from mujoco.egl import egl_ext as EGL + +name = ctypes.util.find_library("EGL") +if name is None: + raise OSError("libEGL not found") + +if platform.system() == "Windows": + _egl = ctypes.WinDLL(name) +else: + _egl = ctypes.CDLL(name, mode=os.RTLD_LOCAL | os.RTLD_NOW) + +addr_make_current = ctypes.cast( + _egl.eglMakeCurrent, ctypes.c_void_p +).value + +ctx = GLContext(max_width=3840, max_height=2160) + +egl_display = mujoco.egl.EGL_DISPLAY.address +egl_context = ctx._context.address + +_cxx.common._bootstrap_egl(addr_make_current, egl_display, egl_context) diff --git a/python/rcs/sim.py b/python/rcs/sim.py index ae7fa550..8049d917 100644 --- a/python/rcs/sim.py +++ b/python/rcs/sim.py @@ -1,11 +1,16 @@ +import atexit import multiprocessing as mp import uuid from logging import getLogger from os import PathLike from pathlib import Path +from threading import Thread from typing import Optional import mujoco as mj +import mujoco.viewer +import rcs.egl_bootstrap +from rcs._core.sim import GuiClient as _GuiClient from rcs._core.sim import Sim as _Sim from rcs._core.sim import ( SimGripper, @@ -15,17 +20,12 @@ SimRobotConfig, SimRobotState, ) -from rcs._core.sim import open_gui_window as _open_gui_window __all__ = ["Sim", "SimRobot", "SimRobotConfig", "SimRobotState", "SimGripper", "SimGripperConfig", "SimGripperState"] logger = getLogger(__name__) -def _start_gui(identifier: str): - _open_gui_window(identifier) - - class Sim(_Sim): def __init__(self, mjmdl: str | PathLike): mjmdl = Path(mjmdl) @@ -40,12 +40,19 @@ def __init__(self, mjmdl: str | PathLike): super().__init__(self.model._address, self.data._address) self._gui_uuid: Optional[str] = None self._mp_context = mp.get_context("spawn") + self._gui_client: Optional[GuiClient] = None + self._gui_thread: Optional[Thread] = None def open_gui(self): if self._gui_uuid is None: self._gui_uuid = "rcs_" + str(uuid.uuid4()) self._start_gui_server(self._gui_uuid) - self._mp_context.Process(target=_start_gui, args=(self._gui_uuid,), daemon=True).start() + atexit.register(self._stop_gui_server) + if self._gui_client is None: + self._gui_client = _GuiClient(self._gui_uuid) + model_byte = self._gui_client.get_model_bytes() + # TODO: load model and create data, pass to a process which calls + # step & sync in a loop def __del__(self): self._stop_gui_server() diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index 762ec53e..72e7733d 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,3 +1,3 @@ add_library(common) -target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp) +target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) target_link_libraries(common PUBLIC Eigen3::Eigen mdl) diff --git a/src/common/utils.cpp b/src/common/utils.cpp new file mode 100644 index 00000000..408735fc --- /dev/null +++ b/src/common/utils.cpp @@ -0,0 +1,25 @@ +#include "utils.h" + +#include + +#include + +namespace rcs { +namespace common { +static PFNEGLMAKECURRENTPROC g_makeCurrent = nullptr; +static EGLDisplay g_display = EGL_NO_DISPLAY; +static EGLSurface g_surface = EGL_NO_SURFACE; +static EGLContext g_context = EGL_NO_CONTEXT; + +void bootstrap_egl(uintptr_t fn_addr, uintptr_t dpy, uintptr_t ctx) { + g_makeCurrent = reinterpret_cast(fn_addr); + g_display = reinterpret_cast(dpy); + g_context = reinterpret_cast(ctx); +} + +void ensure_current() { + if (!g_makeCurrent(g_display, g_surface, g_surface, g_context)) + throw std::runtime_error("eglMakeCurrent failed"); +} +} // namespace common +} // namespace rcs diff --git a/src/common/utils.h b/src/common/utils.h index bb175e2b..53889bcd 100644 --- a/src/common/utils.h +++ b/src/common/utils.h @@ -34,6 +34,9 @@ Eigen::Matrix array2eigen( Eigen::Matrix matrix(array.data()); return matrix; } +void bootstrap_egl(std::uintptr_t fn_addr, std::uintptr_t display, + std::uintptr_t context); +void ensure_current(); } // namespace common } // namespace rcs diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 7ddb4e30..f97b632b 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -140,6 +140,8 @@ PYBIND11_MODULE(_core, m) { // COMMON MODULE auto common = m.def_submodule("common", "common module"); + common.def("_bootstrap_egl", &rcs::common::bootstrap_egl, py::arg("fn_addr"), + py::arg("display"), py::arg("context")); common.def("IdentityTranslation", &rcs::common::IdentityTranslation); common.def("IdentityRotMatrix", &rcs::common::IdentityRotMatrix); common.def("IdentityRotQuatVec", &rcs::common::IdentityRotQuatVec); @@ -540,5 +542,15 @@ PYBIND11_MODULE(_core, m) { .def_property_readonly("_sim", &rcs::sim::SimCameraSet::get_sim) .def("get_timestamp_frameset", &rcs::sim::SimCameraSet::get_timestamp_frameset, py::arg("ts")); - sim.def("open_gui_window", &rcs::sim::open_gui_window, py::arg("uuid")); + py::class_(sim, "GuiClient") + .def(py::init(), py::arg("id")) + .def("get_model_bytes", + [](const rcs::sim::GuiClient &self) { + auto s = self.get_model_bytes(); + return py::bytes(s); + }) + .def("set_model_and_data", [](rcs::sim::GuiClient &self, long m, long d) { + self.set_model_and_data((mjModel *)m, (mjData *)d); + }) + .def("sync", &rcs::sim::GuiClient::sync); } diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index b0b90079..f04d5639 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -2,7 +2,7 @@ add_library(sim) target_sources(sim PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp gui_server.cpp gui_client.cpp ) -target_link_libraries(sim PUBLIC glfw common MuJoCo::MuJoCo mujoco_ui_lib) +target_link_libraries(sim PUBLIC common MuJoCo::MuJoCo) -add_executable(test_sim test.cpp sim.cpp) -target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common glfw) +#add_executable(test_sim test.cpp sim.cpp) +#target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common egl) diff --git a/src/sim/gui.h b/src/sim/gui.h index a9fef4a6..36360492 100644 --- a/src/sim/gui.h +++ b/src/sim/gui.h @@ -33,7 +33,6 @@ static size_t calculate_shm_size(mjModel const* m, mjData const* d) { total_required_size * 0.1; // 10% overhead estimation return total_required_size + estimated_overhead; } -void open_gui_window(std::string& id); struct shm { struct { @@ -66,6 +65,20 @@ class GuiServer { mjData* d; }; +class GuiClient { + public: + GuiClient(const std::string& id); + std::string get_model_bytes() const; + void set_model_and_data(mjModel* m, mjData* d); + void sync(); + + private: + mjModel* m; + mjData* d; + const std::string& id; + struct shm shm; +}; + } // namespace sim } // namespace rcs #endif diff --git a/src/sim/gui_client.cpp b/src/sim/gui_client.cpp index b5fe7c63..3eb37a21 100644 --- a/src/sim/gui_client.cpp +++ b/src/sim/gui_client.cpp @@ -1,38 +1,20 @@ -#include - #include #include +#include -#include "glfw_adapter.h" #include "gui.h" -#include "platform_ui_adapter.h" namespace rcs { namespace sim { using namespace boost::interprocess; -void event_callback(mjuiState*); -class GuiClient { - public: - GuiClient(const std::string& id); - void render_loop(); - mjvCamera cam; - mjvOption opt; - mjvScene scn; - mjModel* m; - mjData* d; - mujoco::PlatformUIAdapter* platform_ui; - - private: - const std::string& id; - struct shm shm; -}; - GuiClient::GuiClient(const std::string& id) : shm{.manager{open_only, id.c_str()}, .state_lock{open_only, (id + STATE_LOCK_POSTFIX).c_str()}, .info_lock{open_only, (id + INFO_LOCK_POSTFIX).c_str()}}, - id{id} { + id{id}, + m{nullptr}, + d{nullptr} { // setup shared memory std::tie(this->shm.info_byte, std::ignore) = this->shm.manager.find(INFO_BYTE); @@ -40,83 +22,26 @@ GuiClient::GuiClient(const std::string& id) this->shm.manager.find(STATE); std::tie(this->shm.model.ptr, this->shm.model.size) = this->shm.manager.find(MODEL); - // initialize model and data from shared memory - mjVFS* vfs = (mjVFS*)mju_malloc(sizeof(mjVFS)); - mj_defaultVFS(vfs); - int failed = mj_addBufferVFS(vfs, "model.mjb", this->shm.model.ptr, - this->shm.model.size); - if (failed != 0) { - mju_free(vfs); - throw std::runtime_error("Could not add file to VFS"); - } - this->m = mj_loadModel("model.mjb", vfs); - mju_free(vfs); - this->d = mj_makeData(m); - this->shm.state_lock.lock_sharable(); - mj_setState(this->m, this->d, this->shm.state.ptr, MJ_PHYSICS_SPEC); - this->shm.state_lock.unlock_sharable(); - mj_step(this->m, this->d); - // start UI window - this->platform_ui = new mujoco::GlfwAdapter(); - this->render_loop(); } -void GuiClient::render_loop() { - mjv_defaultFreeCamera(this->m, &this->cam); - mjv_defaultOption(&this->opt); - mjv_defaultScene(&this->scn); - size_t max_geoms = 2000; - mjv_makeScene(this->m, &this->scn, max_geoms); - mjrRect viewport = {0, 0, 0, 0}; - this->platform_ui->RefreshMjrContext(this->m, mjFONTSCALE_100); - this->platform_ui->SetVSync(true); - this->platform_ui->state().userdata = this; - this->platform_ui->state().nrect = 1; - this->platform_ui->SetEventCallback(event_callback); - while (!this->platform_ui->ShouldCloseWindow()) { - this->platform_ui->PollEvents(); - this->shm.state_lock.lock_sharable(); - mj_setState(this->m, this->d, this->shm.state.ptr, MJ_PHYSICS_SPEC); - this->shm.state_lock.unlock_sharable(); - mj_step(this->m, this->d); - mjv_updateScene(this->m, this->d, &this->opt, NULL, &this->cam, mjCAT_ALL, - &this->scn); - std::tie(viewport.width, viewport.height) = - this->platform_ui->GetFramebufferSize(); - mjr_render(viewport, &this->scn, &this->platform_ui->mjr_context()); - this->shm.info_lock.lock(); - *this->shm.info_byte = true; - this->shm.info_lock.unlock(); - this->platform_ui->SwapBuffers(); - } +std::string GuiClient::get_model_bytes() const { + return std::string(this->shm.model.ptr, this->shm.model.size); } -void open_gui_window(std::string& id) { - rcs::sim::GuiClient gui = rcs::sim::GuiClient(id); - gui.render_loop(); + +void GuiClient::set_model_and_data(mjModel* m, mjData* d) { + this->m = m; + this->d = d; } -void event_callback(mjuiState* state) { - GuiClient* client = static_cast(state->userdata); - mjrRect viewport = {0, 0, 0, 0}; - std::tie(viewport.width, viewport.height) = - client->platform_ui->GetFramebufferSize(); - switch (state->type) { - case mjEVENT_SCROLL: - mjv_moveCamera(client->m, mjMOUSE_ZOOM, 0, -0.02 * state->sy, - &client->scn, &client->cam); - case mjEVENT_MOVE: - // determine action based on mouse button - mjtMouse action; - if (state->right) { - action = state->shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; - } else if (state->left) { - action = state->shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; - } else { - action = mjMOUSE_ZOOM; - } - mjv_moveCamera(client->m, action, state->dx / viewport.height, - -state->dy / viewport.height, &client->scn, &client->cam); - } +void GuiClient::sync() { + // Read new state + this->shm.state_lock.lock_sharable(); + mj_setState(this->m, this->d, this->shm.state.ptr, MJ_PHYSICS_SPEC); + this->shm.state_lock.unlock_sharable(); + // Tell sim that we want new data + this->shm.info_lock.lock(); + *this->shm.info_byte = true; + this->shm.info_lock.unlock(); } } // namespace sim } // namespace rcs diff --git a/src/sim/renderer.cpp b/src/sim/renderer.cpp index 3cf8f3b7..c37ad256 100644 --- a/src/sim/renderer.cpp +++ b/src/sim/renderer.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -9,38 +10,23 @@ namespace rcs { namespace sim { Renderer::Renderer(mjModel* m) : m{m} { - if (!glfwInit()) { - throw std::runtime_error("Could not initialize GLFW"); - } mjv_defaultOption(&this->opt); mjv_defaultScene(&this->scene); size_t max_geoms = 2000; mjv_makeScene(this->m, &this->scene, max_geoms); - this->ctxs = - std::unordered_map>(); + this->ctxs = std::unordered_map(); } Renderer::~Renderer() { for (auto const& [id, ctx] : this->ctxs) { - mjr_freeContext(ctx.second); - glfwDestroyWindow(ctx.first); + mjr_freeContext(ctx); } mjv_freeScene(&this->scene); } void Renderer::register_context(const std::string& id, size_t width, size_t height) { - // create invisible window, single-buffered - glfwWindowHint(GLFW_VISIBLE, 0); - // In record.cc this is false, glfw says in practice you always want true. - glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_FALSE); - GLFWwindow* window = glfwCreateWindow(width, height, "rcs", NULL, NULL); - - if (!window) { - glfwTerminate(); - throw std::runtime_error("Could not create GLFW window"); - } - glfwMakeContextCurrent(window); + common::ensure_current(); mjrContext* ctx = new mjrContext(); mjr_defaultContext(ctx); mjr_makeContext(this->m, ctx, 200); @@ -51,12 +37,12 @@ void Renderer::register_context(const std::string& id, size_t width, "default/window framebuffer" << std::endl; } - this->ctxs[id] = std::pair(window, ctx); + this->ctxs[id] = ctx; } mjrContext* Renderer::get_context(const std::string& id) { - glfwMakeContextCurrent(ctxs[id].first); - return ctxs[id].second; + common::ensure_current(); + return this->ctxs[id]; } } // namespace sim } // namespace rcs diff --git a/src/sim/sim.h b/src/sim/sim.h index b3e272f8..ac48d603 100644 --- a/src/sim/sim.h +++ b/src/sim/sim.h @@ -6,7 +6,6 @@ #include #include -#include "GLFW/glfw3.h" #include "boost/interprocess/managed_shared_memory.hpp" #include "gui.h" #include "mujoco/mujoco.h" @@ -24,7 +23,7 @@ class Renderer { private: mjModel* m; - std::unordered_map> ctxs; + std::unordered_map ctxs; }; struct Config { From 91d71b63dd1809d2967bd17f784a78862d1e26c3 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Fri, 13 Jun 2025 10:26:16 +0200 Subject: [PATCH 2/9] Implemented gui loop in separate process --- python/rcs/sim.py | 44 +++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/python/rcs/sim.py b/python/rcs/sim.py index 8049d917..f95b0090 100644 --- a/python/rcs/sim.py +++ b/python/rcs/sim.py @@ -1,9 +1,13 @@ import atexit +import time import multiprocessing as mp import uuid +from copy import deepcopy from logging import getLogger +from multiprocessing import Event from os import PathLike from pathlib import Path +from tempfile import NamedTemporaryFile from threading import Thread from typing import Optional @@ -26,6 +30,33 @@ logger = getLogger(__name__) +# Target frames per second +FPS = 60.0 +FRAME_DURATION = 1.0 / FPS + + +def gui_loop(model_bytes: bytes, close_event: Event, gui_uuid: str): + with NamedTemporaryFile(mode="wb") as f: + f.write(model_bytes) + model = mujoco.MjModel.from_binary_path(f.name) + data = mujoco.MjData(model) + gui_client = _GuiClient(gui_uuid) + gui_client.set_model_and_data(model._address, data._address) + mujoco.mj_step(model, data) + with mujoco.viewer.launch_passive(model, data) as viewer: + next_frame_time = time.perf_counter() + while not close_event.is_set(): + mujoco.mj_step(model, data) + viewer.sync() + gui_client.sync() + next_frame_time += FRAME_DURATION + sleep_duration = next_frame_time - time.perf_counter() + if sleep_duration > 0: + time.sleep(sleep_duration) + else: + next_frame_time = time.perf_counter() + + class Sim(_Sim): def __init__(self, mjmdl: str | PathLike): mjmdl = Path(mjmdl) @@ -50,9 +81,16 @@ def open_gui(self): atexit.register(self._stop_gui_server) if self._gui_client is None: self._gui_client = _GuiClient(self._gui_uuid) - model_byte = self._gui_client.get_model_bytes() - # TODO: load model and create data, pass to a process which calls - # step & sync in a loop + model_bytes = self._gui_client.get_model_bytes() + ctx = mp.get_context("spawn") + self._stop_event = ctx.Event() + self._gui_process = ctx.Process( + target=gui_loop, + args=(model_bytes, self._stop_event, self._gui_uuid), + ) + self._gui_process.start() def __del__(self): + self._stop_event.set() + self._gui_process.join() self._stop_gui_server() From aef5948c1395e0512e13e78c675add1db4583adf Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Fri, 13 Jun 2025 10:26:43 +0200 Subject: [PATCH 3/9] Stubs --- python/rcs/_core/common.pyi | 1 + python/rcs/_core/sim.pyi | 10 +++++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 42ec9d5b..d650d90c 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -232,6 +232,7 @@ def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Liter def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... def IdentityRotQuatVec() -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... def IdentityTranslation() -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... +def _bootstrap_egl(fn_addr: int, display: int, context: int) -> None: ... def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ... FR3: RobotType # value = diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index e4c6e04e..9a526226 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -12,6 +12,7 @@ import rcs._core.common __all__ = [ "CameraType", "FrameSet", + "GuiClient", "Sim", "SimCameraConfig", "SimCameraSet", @@ -24,7 +25,6 @@ __all__ = [ "default_free", "fixed", "free", - "open_gui_window", "tracking", ] M = typing.TypeVar("M", bound=int) @@ -73,6 +73,12 @@ class FrameSet: @property def timestamp(self) -> float: ... +class GuiClient: + def __init__(self, id: str) -> None: ... + def get_model_bytes(self) -> bytes: ... + def set_model_and_data(self, arg0: int, arg1: int) -> None: ... + def sync(self) -> None: ... + class Sim: def __init__(self, mjmdl: int, mjdata: int) -> None: ... def _start_gui_server(self, id: str) -> None: ... @@ -170,8 +176,6 @@ class SimRobotState(rcs._core.common.RobotState): @property def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... -def open_gui_window(uuid: str) -> None: ... - default_free: CameraType # value = fixed: CameraType # value = free: CameraType # value = From 321e1e7b12e6477cc808fd38e77920f75db1863f Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Fri, 13 Jun 2025 14:45:02 +0200 Subject: [PATCH 4/9] Proper termination for the simulate window --- python/rcs/sim.py | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/python/rcs/sim.py b/python/rcs/sim.py index f95b0090..b2310af6 100644 --- a/python/rcs/sim.py +++ b/python/rcs/sim.py @@ -1,6 +1,6 @@ import atexit -import time import multiprocessing as mp +import time import uuid from copy import deepcopy from logging import getLogger @@ -35,12 +35,13 @@ FRAME_DURATION = 1.0 / FPS -def gui_loop(model_bytes: bytes, close_event: Event, gui_uuid: str): +def gui_loop(gui_uuid: str, close_event: Event): + gui_client = _GuiClient(gui_uuid) + model_bytes = gui_client.get_model_bytes() with NamedTemporaryFile(mode="wb") as f: f.write(model_bytes) model = mujoco.MjModel.from_binary_path(f.name) data = mujoco.MjData(model) - gui_client = _GuiClient(gui_uuid) gui_client.set_model_and_data(model._address, data._address) mujoco.mj_step(model, data) with mujoco.viewer.launch_passive(model, data) as viewer: @@ -69,28 +70,29 @@ def __init__(self, mjmdl: str | PathLike): logger.error(msg) self.data = mj.MjData(self.model) super().__init__(self.model._address, self.data._address) - self._gui_uuid: Optional[str] = None self._mp_context = mp.get_context("spawn") + self._gui_uuid: Optional[str] = None self._gui_client: Optional[GuiClient] = None - self._gui_thread: Optional[Thread] = None + self._gui_process: Optional[mp.Process] = None + self._stop_event: Optional[mp.Event] = None + + def close_gui(self): + if self._stop_event is not None: + self._stop_event.set() + if self._gui_process is not None: + self._gui_process.join() + self._stop_gui_server() def open_gui(self): if self._gui_uuid is None: self._gui_uuid = "rcs_" + str(uuid.uuid4()) self._start_gui_server(self._gui_uuid) - atexit.register(self._stop_gui_server) if self._gui_client is None: - self._gui_client = _GuiClient(self._gui_uuid) - model_bytes = self._gui_client.get_model_bytes() ctx = mp.get_context("spawn") self._stop_event = ctx.Event() self._gui_process = ctx.Process( target=gui_loop, - args=(model_bytes, self._stop_event, self._gui_uuid), + args=(self._gui_uuid, self._stop_event), ) self._gui_process.start() - - def __del__(self): - self._stop_event.set() - self._gui_process.join() - self._stop_gui_server() + atexit.register(self.close_gui) From 9939800695c4694e626518bd757288f107675728 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Fri, 13 Jun 2025 16:36:53 +0200 Subject: [PATCH 5/9] formatting & linter fixes --- python/rcs/egl_bootstrap.py | 19 ++++++++----------- python/rcs/sim.py | 13 ++++++------- src/pybind/rcs.cpp | 7 ++++--- 3 files changed, 18 insertions(+), 21 deletions(-) diff --git a/python/rcs/egl_bootstrap.py b/python/rcs/egl_bootstrap.py index 64090c76..ed773805 100644 --- a/python/rcs/egl_bootstrap.py +++ b/python/rcs/egl_bootstrap.py @@ -1,29 +1,26 @@ import ctypes import ctypes.util import os -import platform import mujoco.egl import rcs._core as _cxx from mujoco.egl import GLContext -from mujoco.egl import egl_ext as EGL name = ctypes.util.find_library("EGL") if name is None: - raise OSError("libEGL not found") + msg = "libEGL not found" + raise OSError(msg) -if platform.system() == "Windows": - _egl = ctypes.WinDLL(name) -else: - _egl = ctypes.CDLL(name, mode=os.RTLD_LOCAL | os.RTLD_NOW) +_egl = ctypes.CDLL(name, mode=os.RTLD_LOCAL | os.RTLD_NOW) -addr_make_current = ctypes.cast( - _egl.eglMakeCurrent, ctypes.c_void_p -).value +addr_make_current = ctypes.cast(_egl.eglMakeCurrent, ctypes.c_void_p).value ctx = GLContext(max_width=3840, max_height=2160) egl_display = mujoco.egl.EGL_DISPLAY.address egl_context = ctx._context.address -_cxx.common._bootstrap_egl(addr_make_current, egl_display, egl_context) + +def bootstrap(): + assert addr_make_current is not None + _cxx.common._bootstrap_egl(addr_make_current, egl_display, egl_context) diff --git a/python/rcs/sim.py b/python/rcs/sim.py index b2310af6..07ccb737 100644 --- a/python/rcs/sim.py +++ b/python/rcs/sim.py @@ -2,13 +2,11 @@ import multiprocessing as mp import time import uuid -from copy import deepcopy from logging import getLogger -from multiprocessing import Event +from multiprocessing.synchronize import Event as EventClass from os import PathLike from pathlib import Path from tempfile import NamedTemporaryFile -from threading import Thread from typing import Optional import mujoco as mj @@ -27,6 +25,7 @@ __all__ = ["Sim", "SimRobot", "SimRobotConfig", "SimRobotState", "SimGripper", "SimGripperConfig", "SimGripperState"] +rcs.egl_bootstrap.bootstrap() logger = getLogger(__name__) @@ -35,7 +34,7 @@ FRAME_DURATION = 1.0 / FPS -def gui_loop(gui_uuid: str, close_event: Event): +def gui_loop(gui_uuid: str, close_event): gui_client = _GuiClient(gui_uuid) model_bytes = gui_client.get_model_bytes() with NamedTemporaryFile(mode="wb") as f: @@ -72,9 +71,9 @@ def __init__(self, mjmdl: str | PathLike): super().__init__(self.model._address, self.data._address) self._mp_context = mp.get_context("spawn") self._gui_uuid: Optional[str] = None - self._gui_client: Optional[GuiClient] = None - self._gui_process: Optional[mp.Process] = None - self._stop_event: Optional[mp.Event] = None + self._gui_client: Optional[_GuiClient] = None + self._gui_process: Optional[mp.context.SpawnProcess] = None + self._stop_event: Optional[EventClass] = None def close_gui(self): if self._stop_event is not None: diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index f97b632b..a6f710ba 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -549,8 +549,9 @@ PYBIND11_MODULE(_core, m) { auto s = self.get_model_bytes(); return py::bytes(s); }) - .def("set_model_and_data", [](rcs::sim::GuiClient &self, long m, long d) { - self.set_model_and_data((mjModel *)m, (mjData *)d); - }) + .def("set_model_and_data", + [](rcs::sim::GuiClient &self, long m, long d) { + self.set_model_and_data((mjModel *)m, (mjData *)d); + }) .def("sync", &rcs::sim::GuiClient::sync); } From 0eaf16176fc8830bf2c4c78af4405b501c2c9ff8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Jun 2025 13:59:57 +0200 Subject: [PATCH 6/9] refactor: moved sim into own folder --- python/rcs/sim/__init__.py | 10 ++++++++++ python/rcs/{ => sim}/egl_bootstrap.py | 0 python/rcs/{ => sim}/sim.py | 14 ++------------ 3 files changed, 12 insertions(+), 12 deletions(-) create mode 100644 python/rcs/sim/__init__.py rename python/rcs/{ => sim}/egl_bootstrap.py (100%) rename python/rcs/{ => sim}/sim.py (90%) diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py new file mode 100644 index 00000000..abe4c689 --- /dev/null +++ b/python/rcs/sim/__init__.py @@ -0,0 +1,10 @@ +from rcs._core.sim import ( + SimGripper, + SimGripperConfig, + SimGripperState, + SimRobot, + SimRobotConfig, + SimRobotState, +) +__all__ = ["Sim", "SimRobot", "SimRobotConfig", "SimRobotState", "SimGripper", "SimGripperConfig", "SimGripperState"] +from rcs.sim.sim import Sim, gui_loop \ No newline at end of file diff --git a/python/rcs/egl_bootstrap.py b/python/rcs/sim/egl_bootstrap.py similarity index 100% rename from python/rcs/egl_bootstrap.py rename to python/rcs/sim/egl_bootstrap.py diff --git a/python/rcs/sim.py b/python/rcs/sim/sim.py similarity index 90% rename from python/rcs/sim.py rename to python/rcs/sim/sim.py index 07ccb737..84764428 100644 --- a/python/rcs/sim.py +++ b/python/rcs/sim/sim.py @@ -11,21 +11,11 @@ import mujoco as mj import mujoco.viewer -import rcs.egl_bootstrap +from rcs.sim import egl_bootstrap from rcs._core.sim import GuiClient as _GuiClient from rcs._core.sim import Sim as _Sim -from rcs._core.sim import ( - SimGripper, - SimGripperConfig, - SimGripperState, - SimRobot, - SimRobotConfig, - SimRobotState, -) -__all__ = ["Sim", "SimRobot", "SimRobotConfig", "SimRobotState", "SimGripper", "SimGripperConfig", "SimGripperState"] - -rcs.egl_bootstrap.bootstrap() +egl_bootstrap.bootstrap() logger = getLogger(__name__) From dc00a4c6489b33fbf8c8bb6c4ac448683d49c428 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Jun 2025 14:12:29 +0200 Subject: [PATCH 7/9] refactor: moved frame rate and improved time tacking --- python/rcs/camera/hw.py | 3 ++- python/rcs/camera/interface.py | 30 ------------------------- python/rcs/utils.py | 41 ++++++++++++++++++++++++++++++++++ 3 files changed, 43 insertions(+), 31 deletions(-) create mode 100644 python/rcs/utils.py diff --git a/python/rcs/camera/hw.py b/python/rcs/camera/hw.py index dc3ae1f5..c0588b4e 100644 --- a/python/rcs/camera/hw.py +++ b/python/rcs/camera/hw.py @@ -9,7 +9,8 @@ import cv2 import numpy as np from rcs._core.common import BaseCameraConfig -from rcs.camera.interface import BaseCameraSet, Frame, FrameSet, SimpleFrameRate +from rcs.camera.interface import BaseCameraSet, Frame, FrameSet +from rcs.utils import SimpleFrameRate class HardwareCamera(typing.Protocol): diff --git a/python/rcs/camera/interface.py b/python/rcs/camera/interface.py index 23a595eb..53f15ab3 100644 --- a/python/rcs/camera/interface.py +++ b/python/rcs/camera/interface.py @@ -1,7 +1,6 @@ import logging from dataclasses import dataclass from datetime import datetime -from time import sleep, time from typing import Any, Protocol from rcs._core.common import BaseCameraConfig @@ -10,35 +9,6 @@ logger.setLevel(logging.INFO) -class SimpleFrameRate: - def __init__(self, frame_rate: int | float): - self.t: float | None = None - self._last_print: float | None = None - self.frame_rate = frame_rate - - def reset(self): - self.t = None - - def __call__(self): - if self.t is None: - self.t = time() - self._last_print = self.t - sleep(1 / self.frame_rate if isinstance(self.frame_rate, int) else self.frame_rate) - return - sleep_time = ( - 1 / self.frame_rate - (time() - self.t) - if isinstance(self.frame_rate, int) - else self.frame_rate - (time() - self.t) - ) - if sleep_time > 0: - sleep(sleep_time) - if self._last_print is None or time() - self._last_print > 10: - self._last_print = time() - logger.info(f"FPS: {1 / (time() - self.t)}") - - self.t = time() - - @dataclass(kw_only=True) class DataFrame: data: Any diff --git a/python/rcs/utils.py b/python/rcs/utils.py new file mode 100644 index 00000000..38773580 --- /dev/null +++ b/python/rcs/utils.py @@ -0,0 +1,41 @@ +import logging +from time import perf_counter, sleep + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class SimpleFrameRate: + def __init__(self, frame_rate: int | float, loop_name: str = "SimpleFrameRate"): + """SimpleFrameRate is a utility class to manage frame rates in a simple way. + It allows you to call it in a loop, and it will sleep the necessary time to maintain the desired frame rate. + + Args: + frame_rate (int | float): The desired frame rate in frames per second (int) or as a time interval in seconds (float). + """ + self.t: float | None = None + self._last_print: float | None = None + self.frame_rate = frame_rate + self.loop_name = loop_name + + def reset(self): + self.t = None + + def __call__(self): + if self.t is None: + self.t = perf_counter() + self._last_print = self.t + sleep(1 / self.frame_rate if isinstance(self.frame_rate, int) else self.frame_rate) + return + sleep_time = ( + 1 / self.frame_rate - (perf_counter() - self.t) + if isinstance(self.frame_rate, int) + else self.frame_rate - (perf_counter() - self.t) + ) + if sleep_time > 0: + sleep(sleep_time) + if self._last_print is None or perf_counter() - self._last_print > 10: + self._last_print = perf_counter() + logger.info(f"FPS {self.loop_name}: {1 / (perf_counter() - self.t)}") + + self.t = perf_counter() From 4cfa7462f90c17ba0db1485b277988c829481136 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Jun 2025 14:13:07 +0200 Subject: [PATCH 8/9] refactor(sim): gui loop to use simple frame rate --- python/rcs/sim/sim.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/python/rcs/sim/sim.py b/python/rcs/sim/sim.py index 84764428..ddd6149b 100644 --- a/python/rcs/sim/sim.py +++ b/python/rcs/sim/sim.py @@ -11,20 +11,21 @@ import mujoco as mj import mujoco.viewer -from rcs.sim import egl_bootstrap from rcs._core.sim import GuiClient as _GuiClient from rcs._core.sim import Sim as _Sim +from rcs.sim import egl_bootstrap +from rcs.utils import SimpleFrameRate egl_bootstrap.bootstrap() logger = getLogger(__name__) # Target frames per second -FPS = 60.0 -FRAME_DURATION = 1.0 / FPS +FPS = 60 def gui_loop(gui_uuid: str, close_event): + frame_rate = SimpleFrameRate(FPS, "gui_loop") gui_client = _GuiClient(gui_uuid) model_bytes = gui_client.get_model_bytes() with NamedTemporaryFile(mode="wb") as f: @@ -39,12 +40,7 @@ def gui_loop(gui_uuid: str, close_event): mujoco.mj_step(model, data) viewer.sync() gui_client.sync() - next_frame_time += FRAME_DURATION - sleep_duration = next_frame_time - time.perf_counter() - if sleep_duration > 0: - time.sleep(sleep_duration) - else: - next_frame_time = time.perf_counter() + frame_rate() class Sim(_Sim): From 78e447d0976a1bf317b3467066faf950db392e42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 16 Jun 2025 14:15:17 +0200 Subject: [PATCH 9/9] doc(sim): explain global vars in egl bootstrap doc and style --- python/rcs/sim/__init__.py | 14 ++++++++++++-- python/rcs/sim/egl_bootstrap.py | 7 +++++++ python/rcs/sim/sim.py | 2 -- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index abe4c689..496ae2ad 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -6,5 +6,15 @@ SimRobotConfig, SimRobotState, ) -__all__ = ["Sim", "SimRobot", "SimRobotConfig", "SimRobotState", "SimGripper", "SimGripperConfig", "SimGripperState"] -from rcs.sim.sim import Sim, gui_loop \ No newline at end of file +from rcs.sim.sim import Sim, gui_loop + +__all__ = [ + "Sim", + "SimRobot", + "SimRobotConfig", + "SimRobotState", + "SimGripper", + "SimGripperConfig", + "SimGripperState", + "gui_loop", +] diff --git a/python/rcs/sim/egl_bootstrap.py b/python/rcs/sim/egl_bootstrap.py index ed773805..6a3d9d5e 100644 --- a/python/rcs/sim/egl_bootstrap.py +++ b/python/rcs/sim/egl_bootstrap.py @@ -1,3 +1,10 @@ +""" +Load the EGL library, create a persistent GLContext, and register it with the C++ backend. + +Globals prevent the library and context from being garbage-collected. +Call `bootstrap()` to complete initialization. +""" + import ctypes import ctypes.util import os diff --git a/python/rcs/sim/sim.py b/python/rcs/sim/sim.py index ddd6149b..9fc5feb2 100644 --- a/python/rcs/sim/sim.py +++ b/python/rcs/sim/sim.py @@ -1,6 +1,5 @@ import atexit import multiprocessing as mp -import time import uuid from logging import getLogger from multiprocessing.synchronize import Event as EventClass @@ -35,7 +34,6 @@ def gui_loop(gui_uuid: str, close_event): gui_client.set_model_and_data(model._address, data._address) mujoco.mj_step(model, data) with mujoco.viewer.launch_passive(model, data) as viewer: - next_frame_time = time.perf_counter() while not close_event.is_set(): mujoco.mj_step(model, data) viewer.sync()