diff --git a/CMakeLists.txt b/CMakeLists.txt index 48e4b229..e99caeb8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,7 +44,6 @@ set(BUILD_DOCUMENTATION OFF) include(FetchContent) find_package(Eigen3 REQUIRED) -find_package(glfw3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(MuJoCo REQUIRED) @@ -67,29 +66,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) include(compile_scenes) 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 = 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/sim.py b/python/rcs/sim.py deleted file mode 100644 index ae7fa550..00000000 --- a/python/rcs/sim.py +++ /dev/null @@ -1,51 +0,0 @@ -import multiprocessing as mp -import uuid -from logging import getLogger -from os import PathLike -from pathlib import Path -from typing import Optional - -import mujoco as mj -from rcs._core.sim import Sim as _Sim -from rcs._core.sim import ( - SimGripper, - SimGripperConfig, - SimGripperState, - SimRobot, - 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) - if mjmdl.suffix == ".xml": - self.model = mj.MjModel.from_xml_path(str(mjmdl)) - elif mjmdl.suffix == ".mjb": - self.model = mj.MjModel.from_binary_path(str(mjmdl)) - else: - msg = f"Filetype {mjmdl.suffix} is unknown" - 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") - - 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() - - def __del__(self): - self._stop_gui_server() diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py new file mode 100644 index 00000000..496ae2ad --- /dev/null +++ b/python/rcs/sim/__init__.py @@ -0,0 +1,20 @@ +from rcs._core.sim import ( + SimGripper, + SimGripperConfig, + SimGripperState, + SimRobot, + SimRobotConfig, + SimRobotState, +) +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 new file mode 100644 index 00000000..6a3d9d5e --- /dev/null +++ b/python/rcs/sim/egl_bootstrap.py @@ -0,0 +1,33 @@ +""" +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 + +import mujoco.egl +import rcs._core as _cxx +from mujoco.egl import GLContext + +name = ctypes.util.find_library("EGL") +if name is None: + msg = "libEGL not found" + raise OSError(msg) + +_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 + + +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/sim.py b/python/rcs/sim/sim.py new file mode 100644 index 00000000..9fc5feb2 --- /dev/null +++ b/python/rcs/sim/sim.py @@ -0,0 +1,81 @@ +import atexit +import multiprocessing as mp +import uuid +from logging import getLogger +from multiprocessing.synchronize import Event as EventClass +from os import PathLike +from pathlib import Path +from tempfile import NamedTemporaryFile +from typing import Optional + +import mujoco as mj +import mujoco.viewer +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 + + +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: + f.write(model_bytes) + model = mujoco.MjModel.from_binary_path(f.name) + data = mujoco.MjData(model) + gui_client.set_model_and_data(model._address, data._address) + mujoco.mj_step(model, data) + with mujoco.viewer.launch_passive(model, data) as viewer: + while not close_event.is_set(): + mujoco.mj_step(model, data) + viewer.sync() + gui_client.sync() + frame_rate() + + +class Sim(_Sim): + def __init__(self, mjmdl: str | PathLike): + mjmdl = Path(mjmdl) + if mjmdl.suffix == ".xml": + self.model = mj.MjModel.from_xml_path(str(mjmdl)) + elif mjmdl.suffix == ".mjb": + self.model = mj.MjModel.from_binary_path(str(mjmdl)) + else: + msg = f"Filetype {mjmdl.suffix} is unknown" + logger.error(msg) + self.data = mj.MjData(self.model) + 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.context.SpawnProcess] = None + self._stop_event: Optional[EventClass] = 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) + if self._gui_client is None: + ctx = mp.get_context("spawn") + self._stop_event = ctx.Event() + self._gui_process = ctx.Process( + target=gui_loop, + args=(self._gui_uuid, self._stop_event), + ) + self._gui_process.start() + atexit.register(self.close_gui) 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() 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..a6f710ba 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,16 @@ 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 {