refactor(mujoco): split world and viewer runtime

cmvr-es/config/devices/mujoco/mujoco_viewer.pb.txt

@@ -0,0 +1,7 @@
+viewers {
+  id: "mujoco_viewer"
+  world_id: "mujoco_world"
+  camera_distance: 3.0
+  camera_azimuth: 0.0
+  camera_elevation: -30.0
+}

cmvr-es/config/devices/mujoco/mujoco_world.pb.txt

@@ -0,0 +1,7 @@
+worlds {
+  id: "mujoco_world"
+  model_path: "model/xiaoyan_description/dual_arm.xml"
+  timestep_s: 0.001
+  realtime_factor: 1.0
+  require_actuator: true
+}

cmvr-es/simulate/mujoco/CMakeLists.txt

@@ -1 +1,2 @@
-add_subdirectory(mujoco_viewer)
+add_subdirectory(mujoco_world)
+add_subdirectory(mujoco_viewer)

cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt

@@ -1,14 +1,15 @@
-file(GLOB SRC
+add_library(mujoco_viewer SHARED
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer.cpp
+        src/mujoco_viewer.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cc
 )
 
-add_library(mujoco_viewer SHARED ${SRC})
-
 
 target_include_directories(mujoco_viewer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(mujoco_viewer PUBLIC
+        cmvr_es::proto
+        cmvr_es::common
+        cmvr_es::mujoco_world
+        simulate
         mujoco
         glfw
         GL
@@ -24,25 +25,14 @@ install(TARGETS mujoco_viewer LIBRARY DESTINATION lib)
 ## --------------------------------------------------------
 ## Unit test
 ## --------------------------------------------------------
-#find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+add_executable(mujoco_viewer_test
-#find_package(Python3 COMPONENTS NumPy)
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer_test.cpp
-#
+)
-#
-#add_executable(mujoco_viewer_test
-#        ${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer_test.cpp
-#)
-#
-#
-#target_link_libraries(mujoco_viewer_test
-#        PRIVATE
-#        gtest
-#        gtest_main
-#        pthread
-#        glog
-#        cmvr_es::mujoco_viewer
-#        cmvr_es::algorithms::controller
-#        Python3::Python
-#        Python3::NumPy
-#)
-
 
+target_link_libraries(mujoco_viewer_test
+        PRIVATE
+        gtest
+        gtest_main
+        cmvr_es::mujoco_viewer
+        cmvr_es::mujoco_world
+)

cmvr-es/simulate/mujoco/mujoco_viewer/include/array_safety.h

@@ -1,105 +0,0 @@
-// Copyright 2021 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_SAMPLE_ARRAY_SAFETY_H_
-#define MUJOCO_SAMPLE_ARRAY_SAFETY_H_
-
-#include <algorithm>
-#include <cstdarg>
-#include <cstddef>
-#include <cstdio>
-#include <cstring>
-
-// Provides safe alternatives to the sizeof() operator and standard library functions for handling
-// null-terminated (C-style) strings in raw char arrays.
-//
-// These functions make use of compile-time array sizes to limit read and write operations to within
-// the array bounds. They are designed to trigger a compile error if the array size cannot be
-// determined at compile time (e.g. when an array has decayed into a pointer).
-//
-// They do not perform runtime bound checks.
-
-namespace mujoco {
-namespace sample_util {
-
-// returns sizeof(arr)
-// use instead of sizeof() to avoid unintended array-to-pointer decay
-template <typename T, int N>
-static constexpr std::size_t sizeof_arr(const T(&arr)[N]) {
-  return sizeof(arr);
-}
-
-// like std::strcmp but it will not read beyond the bound of either lhs or rhs
-template <std::size_t N1, std::size_t N2>
-static inline int strcmp_arr(const char (&lhs)[N1], const char (&rhs)[N2]) {
-  return std::strncmp(lhs, rhs, std::min(N1, N2));
-}
-
-// like std::strlen but it will not read beyond the bound of str
-// if str is not null-terminated, returns sizeof(str)
-template <std::size_t N>
-static inline std::size_t strlen_arr(const char (&str)[N]) {
-  for (std::size_t i = 0; i < N; ++i) {
-    if (str[i] == '\0') {
-      return i;
-    }
-  }
-  return N;
-}
-
-// like std::sprintf but will not write beyond the bound of dest
-// dest is guaranteed to be null-terminated
-template <std::size_t N>
-static inline int sprintf_arr(char (&dest)[N], const char* format, ...) {
-  std::va_list vargs;
-  va_start(vargs, format);
-  int retval = std::vsnprintf(dest, N, format, vargs);
-  va_end(vargs);
-  return retval;
-}
-
-// like std::strcat but will not write beyond the bound of dest
-// dest is guaranteed to be null-terminated
-template <std::size_t N>
-static inline char* strcat_arr(char (&dest)[N], const char* src) {
-  const std::size_t dest_len = strlen_arr(dest);
-  const std::size_t dest_size = sizeof_arr(dest);
-  for (std::size_t i = dest_len; i < dest_size; ++i) {
-    dest[i] = src[i - dest_len];
-    if (!dest[i]) {
-      break;
-    }
-  }
-  dest[dest_size - 1] = '\0';
-  return dest;
-}
-
-// like std::strcpy but won't write beyond the bound of dest
-// dest is guaranteed to be null-terminated
-template <std::size_t N>
-static inline char* strcpy_arr(char (&dest)[N], const char* src) {
-  {
-    std::size_t i = 0;
-    for (; src[i] && i < N - 1; ++i) {
-      dest[i] = src[i];
-    }
-    dest[i] = '\0';
-  }
-  return &dest[0];
-}
-
-}  // namespace sample_util
-}  // namespace mujoco
-
-#endif  // MUJOCO_SAMPLE_ARRAY_SAFETY_H_

cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_adapter.h

@@ -1,78 +0,0 @@
-// Copyright 2023 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_SIMULATE_GLFW_ADAPTER_H_
-#define MUJOCO_SIMULATE_GLFW_ADAPTER_H_
-
-#include <utility>
-
-#include <GLFW/glfw3.h>
-#include <mujoco/mujoco.h>
-#include "platform_ui_adapter.h"
-
-#ifdef __APPLE__
-#include <optional>
-#include "glfw_corevideo.h"
-#endif
-
-namespace mujoco {
-class GlfwAdapter : public PlatformUIAdapter {
- public:
-  GlfwAdapter();
-  ~GlfwAdapter() override;
-
-  std::pair<double, double> GetCursorPosition() const override;
-  double GetDisplayPixelsPerInch() const override;
-  std::pair<int, int> GetFramebufferSize() const override;
-  std::pair<int, int> GetWindowSize() const override;
-  bool IsGPUAccelerated() const override;
-  void PollEvents() override;
-  void SetClipboardString(const char* text) override;
-  void SetVSync(bool enabled) override;
-  void SetWindowTitle(const char* title) override;
-  bool ShouldCloseWindow() const override;
-  void SwapBuffers() override;
-  void ToggleFullscreen() override;
-
-  bool IsLeftMouseButtonPressed() const override;
-  bool IsMiddleMouseButtonPressed() const override;
-  bool IsRightMouseButtonPressed() const override;
-
-  bool IsAltKeyPressed() const override;
-  bool IsCtrlKeyPressed() const override;
-  bool IsShiftKeyPressed() const override;
-
-  bool IsMouseButtonDownEvent(int act) const override;
-  bool IsKeyDownEvent(int act) const override;
-
-  int TranslateKeyCode(int key) const override;
-  mjtButton TranslateMouseButton(int button) const override;
-
- private:
-  GLFWvidmode vidmode_;
-  GLFWwindow* window_;
-
-  // store last window information when going to full screen
-  std::pair<int, int> window_pos_;
-  std::pair<int, int> window_size_;
-
-#ifdef __APPLE__
-  // Workaround for perpertually broken OpenGL VSync on macOS,
-  // most recently https://github.com/glfw/glfw/issues/2249.
-  std::optional<GlfwCoreVideo> core_video_;
-#endif
-};
-}  // namespace mujoco
-
-#endif  // MUJOCO_SIMULATE_GLFW_ADAPTER_H_

cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_corevideo.h

@@ -1,57 +0,0 @@
-// Copyright 2023 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_SIMULATE_GLFW_COREVIDEO_H_
-#define MUJOCO_SIMULATE_GLFW_COREVIDEO_H_
-
-#ifndef __APPLE__
-#error "This header only works on macOS."
-#endif
-
-#include <atomic>
-#include <condition_variable>
-#include <mutex>
-
-#include "glfw_dispatch.h"
-
-#ifdef __OBJC__
-#import <CoreVideo/CoreVideo.h>
-#else
-typedef void* CVDisplayLinkRef;
-#endif
-
-// Workaround for perpertually broken OpenGL VSync on macOS,
-// most recently https://github.com/glfw/glfw/issues/2249.
-namespace mujoco {
-class GlfwCoreVideo {
- public:
-  GlfwCoreVideo(GLFWwindow* window);
-  ~GlfwCoreVideo();
-
-  void WaitForDisplayRefresh();
-  int DisplayLinkCallback();
-  void UpdateDisplayLink();
-
- private:
-  GLFWwindow* window_;
-  CVDisplayLinkRef display_link_;
-
-  std::atomic_bool waiting_;
-  std::mutex mu_;
-  std::condition_variable cond_;
-};
-}  // namespace mujoco
-
-
-#endif  // MUJOCO_SIMULATE_GLFW_COREVIDEO_H_

cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h

@@ -1,77 +0,0 @@
-// Copyright 2022 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_SIMULATE_GLFW_DISPATCH_H_
-#define MUJOCO_SIMULATE_GLFW_DISPATCH_H_
-
-#include <GLFW/glfw3.h>
-
-#ifdef __APPLE__
-#define GLFW_EXPOSE_NATIVE_NSGL
-#include <GLFW/glfw3native.h>
-#endif
-
-namespace mujoco {
-// Dynamic dispatch table for GLFW functions required by Simulate.
-// This allows us to use GLFW without introducing a link-time dependency on the
-// library, which is useful e.g. when using GLFW via Python.
-struct Glfw {
-#define mjGLFW_DECLARE_SYMBOL(func) decltype(&::func) func
-  // go/keep-sorted start
-  mjGLFW_DECLARE_SYMBOL(glfwCreateWindow);
-  mjGLFW_DECLARE_SYMBOL(glfwDestroyWindow);
-  mjGLFW_DECLARE_SYMBOL(glfwGetCursorPos);
-  mjGLFW_DECLARE_SYMBOL(glfwGetFramebufferSize);
-  mjGLFW_DECLARE_SYMBOL(glfwGetKey);
-  mjGLFW_DECLARE_SYMBOL(glfwGetMonitorPhysicalSize);
-  mjGLFW_DECLARE_SYMBOL(glfwGetMouseButton);
-  mjGLFW_DECLARE_SYMBOL(glfwGetPrimaryMonitor);
-  mjGLFW_DECLARE_SYMBOL(glfwGetTime);
-  mjGLFW_DECLARE_SYMBOL(glfwGetVideoMode);
-  mjGLFW_DECLARE_SYMBOL(glfwGetWindowMonitor);
-  mjGLFW_DECLARE_SYMBOL(glfwGetWindowPos);
-  mjGLFW_DECLARE_SYMBOL(glfwGetWindowSize);
-  mjGLFW_DECLARE_SYMBOL(glfwGetWindowUserPointer);
-  mjGLFW_DECLARE_SYMBOL(glfwInit);
-  mjGLFW_DECLARE_SYMBOL(glfwMakeContextCurrent);
-  mjGLFW_DECLARE_SYMBOL(glfwPollEvents);
-  mjGLFW_DECLARE_SYMBOL(glfwSetClipboardString);
-  mjGLFW_DECLARE_SYMBOL(glfwSetCursorPosCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetDropCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetKeyCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetMouseButtonCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetScrollCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetWindowMonitor);
-  mjGLFW_DECLARE_SYMBOL(glfwSetWindowRefreshCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetWindowSizeCallback);
-  mjGLFW_DECLARE_SYMBOL(glfwSetWindowTitle);
-  mjGLFW_DECLARE_SYMBOL(glfwSetWindowUserPointer);
-  mjGLFW_DECLARE_SYMBOL(glfwSwapBuffers);
-  mjGLFW_DECLARE_SYMBOL(glfwSwapInterval);
-  mjGLFW_DECLARE_SYMBOL(glfwTerminate);
-  mjGLFW_DECLARE_SYMBOL(glfwWindowHint);
-  mjGLFW_DECLARE_SYMBOL(glfwWindowShouldClose);
-  // go/keep-sorted end
-
-#ifdef __APPLE__
-  mjGLFW_DECLARE_SYMBOL(glfwGetNSGLContext);
-#endif
-
-#undef mjGLFW_DECLARE_SYMBOL
-};
-
-const struct Glfw& Glfw(void* dlhandle = nullptr);
-}  // namespace mujoco
-
-#endif  // MUJOCO_SIMULATE_GLFW_DISPATCH_H_

cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h

@@ -10,16 +10,19 @@
 #include <thread>
 #include <vector>
 
+#include "cmvr/config/camera_config/camera_config.pb.h"
+#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h"
+#include "devices/abstract_device.h"
 #include "mujoco/mujoco.h"
-#include "simulate/mujoco/mujoco_viewer/include/simulate.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
-#include "common/base/constants.h"
+#include <simulate/simulate.h>
 
 namespace cmvr {
     class PiPGlfwAdapter;
     class MuJocoViewer {
         friend class PiPGlfwAdapter;
     public:
-        explicit MuJocoViewer(const char *model_path);
+        explicit MuJocoViewer(std::shared_ptr<simulate::MujocoWorld> world);
 
         ~MuJocoViewer();
 
@@ -32,8 +35,8 @@ namespace cmvr {
         void setRunning(bool running);
         void requestStop();
 
-        mjModel *model() const { return m_; }
+        mjModel *model() const;
-        mjData *data() const { return d_; }
+        mjData *data() const;
 
         // 同一窗口画中画：显示模型内固定相机视角（像素坐标）
         void enablePiPCamera(const char *camera_name);
@@ -42,6 +45,13 @@ namespace cmvr {
                              int bottom,
                              int width,
                              int height);
+        void enablePiPCamera(const char *camera_name,
+                             int left,
+                             int bottom,
+                             int display_width,
+                             int display_height,
+                             int render_width,
+                             int render_height);
         void disablePiPCamera();
         // 获取 PiP 相机 RGB+Depth（Depth 已线性化为米）
         // depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
@@ -54,22 +64,7 @@ namespace cmvr {
         // 只拿 frame_id，便于 physics 线程判断是否新帧
         uint64_t getPiPCameraFrameId() const;
 
-    protected:
+    public:
-        // 每次 mj_step 前，physics 线程回调控制逻辑
-        virtual void controlCallback(mjModel *m, mjData *d) {
-            UNUSED_VARIABLE(m, d);
-        }
-
-        // 点击reset 会调用这个函数
-        virtual void onReset(mjModel *m, mjData *d) {
-            UNUSED_VARIABLE(m, d);
-        }
-
-        // 只在第一次进控制循环时调用一次
-        virtual void initOnce(mjModel *m, mjData *d) {
-            UNUSED_VARIABLE(m, d);
-        }
-
         void setupCamera(double distance = 3.0,
                          double azimuth = 0.0,
                          double elevation = -30.0); // 设置相机视角参数
@@ -85,39 +80,27 @@ namespace cmvr {
 
     private:
         void renderPiP();
-        void initSim(); // 只创建 Simulate，不 load
+        void initSim();
-        void physicsThreadFunc(); // 加载模型 + 物理循环
+        void syncThreadFunc();
-        void physicsLoop(); // 真正的一步一步仿真
-        void controlDispatch(mjModel *m, mjData *d);
-
-
-        static mjModel *LoadModelSimple(const char *file,
-                                        char *load_error,
-                                        int error_sz);
 
     private:
-        std::string model_path_;
+        std::shared_ptr<simulate::MujocoWorld> world_;
-
-        mjModel *m_ = nullptr;
-        mjData *d_ = nullptr;
 
         mjvCamera cam_;
         mjvOption opt_;
         mjvPerturb pert_;
 
         std::unique_ptr<mujoco::Simulate> sim_;
-        std::thread physics_thread_;
+        std::thread sync_thread_;
-        std::atomic<bool> physics_started_{false};
-
-
-        bool inited_ = false;
-        mjtNum last_time_ = 0.0;
 
         bool pip_enabled_ = false;
         std::string pip_camera_name_;
         int pip_camera_id_ = -1;
         int pip_width_ = 320;
         int pip_height_ = 240;
+        int pip_render_width_ = 320;
+        int pip_render_height_ = 240;
+        bool pip_render_size_warning_logged_ = false;
         int pip_margin_ = 10;
         bool pip_custom_pos_ = false;
         int pip_left_ = 0;
@@ -135,4 +118,35 @@ namespace cmvr {
         uint64_t pip_frame_id_ = 0;        // 新增：帧序号
 
     };
+
+    class MujocoViewerDevice final : public device::AbstractDevice {
+    public:
+        explicit MujocoViewerDevice(config::MujocoViewerConfig config);
+        ~MujocoViewerDevice() override;
+
+        device::DeviceKind kind() const noexcept override { return device::DeviceKind::MujocoViewer; }
+        std::string typeName() const override { return "MujocoViewerDevice"; }
+
+        bool init() override;
+        bool start() override;
+        bool stop() override;
+        bool runOnMainThread();
+        bool setPiPCameraConfig(const config::MujocoCameraConfig& camera_config);
+        bool getPiPCameraRGBD(std::vector<unsigned char>& rgb,
+                              std::vector<float>& depth,
+                              int& width,
+                              int& height,
+                              uint64_t& frame_id) const;
+
+    private:
+        config::MujocoViewerConfig config_;
+        config::MujocoCameraConfig pip_camera_config_;
+        std::shared_ptr<simulate::MujocoWorld> world_;
+        std::unique_ptr<MuJocoViewer> viewer_;
+        std::thread viewer_thread_;
+        mutable std::mutex mtx_;
+        bool has_pip_camera_config_ = false;
+        bool running_ = false;
+        bool stop_requested_ = false;
+    };
 } // namespace cmvr

cmvr-es/simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h

@@ -1,101 +0,0 @@
-// Copyright 2023 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_
-#define MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_
-
-#include <utility>
-
-#include <mujoco/mujoco.h>
-
-namespace mujoco {
-class PlatformUIAdapter {
- public:
-  virtual ~PlatformUIAdapter() = default;
-
-  inline mjuiState& state() { return state_; }
-  inline const mjuiState& state() const { return state_; }
-
-  inline mjrContext& mjr_context() { return con_; }
-  inline const mjrContext& mjr_context() const { return con_; }
-
-  inline void SetEventCallback(void (*event_callback)(mjuiState*)) {
-    event_callback_ = event_callback;
-  }
-
-  inline void SetLayoutCallback(void (*layout_callback)(mjuiState*)) {
-    layout_callback_ = layout_callback;
-  }
-
-  // Optionally overridable function to (re)create an mjrContext for an mjModel
-  virtual bool RefreshMjrContext(const mjModel* m, int fontscale);
-
-  virtual bool EnsureContextSize();
-
-  // Pure virtual functions to be implemented by individual adapters
-  virtual std::pair<double, double> GetCursorPosition() const = 0;
-  virtual double GetDisplayPixelsPerInch() const = 0;
-  virtual std::pair<int, int> GetFramebufferSize() const = 0;
-  virtual std::pair<int, int> GetWindowSize() const = 0;
-  virtual bool IsGPUAccelerated() const = 0;
-  virtual void PollEvents() = 0;
-  virtual void SetClipboardString(const char* text) = 0;
-  virtual void SetVSync(bool enabled) = 0;
-  virtual void SetWindowTitle(const char* title) = 0;
-  virtual bool ShouldCloseWindow() const = 0;
-  virtual void SwapBuffers() = 0;
-  virtual void ToggleFullscreen() = 0;
-
-  virtual bool IsLeftMouseButtonPressed() const = 0;
-  virtual bool IsMiddleMouseButtonPressed() const = 0;
-  virtual bool IsRightMouseButtonPressed() const = 0;
-
-  virtual bool IsAltKeyPressed() const = 0;
-  virtual bool IsCtrlKeyPressed() const = 0;
-  virtual bool IsShiftKeyPressed() const = 0;
-
-  virtual bool IsMouseButtonDownEvent(int act) const = 0;
-  virtual bool IsKeyDownEvent(int act) const = 0;
-
-  virtual int TranslateKeyCode(int key) const = 0;
-  virtual mjtButton TranslateMouseButton(int button) const = 0;
-
- protected:
-  PlatformUIAdapter();
-  void FreeMjrContext();
-
-  // Event handlers
-  void OnFilesDrop(int count, const char** paths);
-  virtual void OnKey(int key, int scancode, int act);
-  void OnMouseButton(int button, int act);
-  void OnMouseMove(double x, double y);
-  void OnScroll(double xoffset, double yoffset);
-  void OnWindowRefresh();
-  void OnWindowResize(int width, int height);
-
-  mjuiState state_;
-  int last_key_;
-  void (*event_callback_)(mjuiState*);
-  void (*layout_callback_)(mjuiState*);
-
-  mjrContext con_;
-  const mjModel* last_model_ = nullptr;
-  int last_fontscale_ = -1;
-
- private:
-  void UpdateMjuiState();
-};
-}  // namespace mujoco
-
-#endif  // MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_

cmvr-es/simulate/mujoco/mujoco_viewer/include/simulate.h

@@ -1,353 +0,0 @@
-// Copyright 2021 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MUJOCO_SIMULATE_SIMULATE_H_
-#define MUJOCO_SIMULATE_SIMULATE_H_
-
-#include <atomic>
-#include <chrono>
-#include <condition_variable>
-#include <memory>
-#include <mutex>
-#include <optional>
-#include <ratio>
-#include <string>
-#include <tuple>
-#include <utility>
-#include <vector>
-
-#include <mujoco/mjui.h>
-#include <mujoco/mujoco.h>
-#include "platform_ui_adapter.h"
-
-namespace mujoco {
-
-// The viewer itself doesn't require a reentrant mutex, however we use it in
-// order to provide a Python sync API that doesn't require separate locking
-// (since sync is by far the most common operation), but that also won't
-// deadlock if called when a lock is already held by the user script on the
-// same thread.
-class SimulateMutex : public std::recursive_mutex {};
-using MutexLock = std::unique_lock<std::recursive_mutex>;
-
-// Simulate states not contained in MuJoCo structures
-class Simulate {
- public:
-  using Clock = std::chrono::steady_clock;
-  static_assert(std::ratio_less_equal_v<Clock::period, std::milli>);
-
-  static constexpr int kMaxGeom = 100000;
-
-  // create object and initialize the simulate ui
-  Simulate(
-      std::unique_ptr<PlatformUIAdapter> platform_ui_adapter,
-      mjvCamera* cam, mjvOption* opt, mjvPerturb* pert, bool is_passive);
-
-  // Synchronize state with UI inputs, and update visualization.  If state_only
-  // is false mjData and mjModel will be updated, otherwise only the subset of
-  // mjData corresponding to mjSTATE_INTEGRATION will be synced.
-  void Sync(bool state_only = false);
-
-  void UpdateHField(int hfieldid);
-  void UpdateMesh(int meshid);
-  void UpdateTexture(int texid);
-
-  // Request that the Simulate UI display a "loading" message
-  // Called prior to Load or LoadMessageClear
-  void LoadMessage(const char* displayed_filename);
-
-  // Request that the Simulate UI thread render a new model
-  void Load(mjModel* m, mjData* d, const char* displayed_filename);
-
-  // Clear the loading message
-  // Can be called instead of Load to clear the message without
-  // requesting the UI load a model
-  void LoadMessageClear(void);
-
-  // functions below are used by the renderthread
-  // load mjb or xml model that has been requested by load()
-  void LoadOnRenderThread();
-
-  // render the ui to the window
-  void Render();
-
-  // loop to render the UI (must be called from main thread because of MacOS)
-  void RenderLoop();
-
-  // add state to history buffer
-  void AddToHistory();
-
-  // inject control noise
-  void InjectNoise(int key);
-
-  // constants
-  static constexpr int kMaxFilenameLength = 1000;
-
-  // whether the viewer is operating in passive mode, where it cannot assume
-  // that it has exclusive access to mjModel, mjData, and various mjv objects
-  bool is_passive_ = false;
-
-  // model and data to be visualized
-  mjModel* mnew_ = nullptr;
-  mjData* dnew_ = nullptr;
-
-  mjModel* m_ = nullptr;
-  mjData* d_ = nullptr;
-
-  int ncam_ = 0;
-  int nkey_ = 0;
-  int state_size_ = 0;      // number of mjtNums in a history buffer state
-  int nhistory_ = 0;        // number of states saved in history buffer
-  int history_cursor_ = 0;  // cursor pointing at last saved state
-
-  std::vector<int> body_parentid_;
-
-  std::vector<int> jnt_type_;
-  std::vector<int> jnt_group_;
-  std::vector<int> jnt_qposadr_;
-  std::vector<std::optional<std::pair<mjtNum, mjtNum>>> jnt_range_;
-  std::vector<std::string> jnt_names_;
-
-  std::vector<int> actuator_group_;
-  std::vector<std::optional<std::pair<mjtNum, mjtNum>>> actuator_ctrlrange_;
-  std::vector<std::string> actuator_names_;
-
-  std::vector<std::string> equality_names_;
-
-  std::vector<mjtNum> history_;  // history buffer (nhistory x state_size)
-
-  // mjModel and mjData fields that can be modified by the user through the GUI
-  std::vector<mjtNum> qpos_;
-  std::vector<mjtNum> qpos_prev_;
-  std::vector<mjtNum> ctrl_;
-  std::vector<mjtNum> ctrl_prev_;
-  std::vector<mjtByte> eq_active_;
-  std::vector<mjtByte> eq_active_prev_;
-
-  // in passive mode the user owns m_ and d_, these "passive" instances are
-  // owned by Simulate, updated from the user by the Sync() method
-  mjModel* m_passive_ = nullptr;
-  mjData* d_passive_ = nullptr;
-  std::vector<mjvGeom> user_scn_geoms_;
-
-  mjOption mjopt_prev_;
-  mjVisual mjvis_prev_;
-  mjStatistic mjstat_prev_;
-  mjvOption opt_prev_;
-  mjvCamera cam_prev_;
-
-  int warn_vgeomfull_prev_;
-
-  // pending GUI-driven actions, to be applied at the next call to Sync
-  struct {
-    std::optional<std::string> save_xml;
-    std::optional<std::string> save_mjb;
-    std::optional<std::string> print_model;
-    std::optional<std::string> print_data;
-    bool reset;
-    bool align;
-    bool copy_key;
-    bool copy_key_full_precision;
-    bool load_from_history;
-    bool load_key;
-    bool save_key;
-    bool zero_ctrl;
-    int newperturb;
-    bool select;
-    mjuiState select_state;
-    bool ui_update_simulation;
-    bool ui_update_physics;
-    bool ui_update_rendering;
-    bool ui_update_visualization;
-    bool ui_update_joint;
-    bool ui_update_ctrl;
-    bool ui_update_equality;
-    bool ui_remake_ctrl;
-  } pending_ = {};
-
-  SimulateMutex mtx;
-  std::condition_variable_any cond_loadrequest;
-
-  int frames_ = 0;
-  std::chrono::time_point<Clock> last_fps_update_;
-  double fps_ = 0;
-
-  // options
-  int spacing = 0;
-  int color = 0;
-  int font = 0;
-  int ui0_enable = 1;
-  int ui1_enable = 1;
-  int help = 0;
-  int info = 0;
-  int profiler = 0;
-  int sensor = 0;
-  int pause_update = 0;
-  int fullscreen = 0;
-  int vsync = 1;
-  int busywait = 0;
-
-  // keyframe index
-  int key = -1;
-
-  // index of history-scrubber slider
-  int scrub_index = 0;
-
-  // simulation
-  int run = 1;
-
-  // atomics for cross-thread messages
-  std::atomic_int exitrequest = 0;
-  std::atomic_int droploadrequest = 0;
-  std::atomic_int screenshotrequest = 0;
-  std::atomic_int uiloadrequest = 0;
-  std::atomic_int newfigurerequest = 0;
-  std::atomic_int newtextrequest = 0;
-  std::atomic_int newimagerequest = 0;
-
-  // loadrequest
-  //   3: display a loading message
-  //   2: render thread asked to update its model
-  //   1: showing "loading" label, about to load
-  //   0: model loaded or no load requested.
-  int loadrequest = 0;
-
-  // strings
-  char load_error[kMaxFilenameLength] = "";
-  char dropfilename[kMaxFilenameLength] = "";
-  char filename[kMaxFilenameLength] = "";
-  char previous_filename[kMaxFilenameLength] = "";
-
-  // time synchronization
-  int real_time_index = 0;
-  bool speed_changed = true;
-  float measured_slowdown = 1.0;
-  // logarithmically spaced real-time slow-down coefficients (percent)
-  static constexpr float percentRealTime[] = {
-      100, 80, 66,  50,  40, 33,  25,  20, 16,  13,
-      10,  8,  6.6, 5.0, 4,  3.3, 2.5, 2,  1.6, 1.3,
-      1,  .8, .66, .5,  .4, .33, .25, .2, .16, .13,
-     .1
-  };
-
-  // control noise
-  double ctrl_noise_std = 0.0;
-  double ctrl_noise_rate = 0.0;
-
-  // watch
-  char field[mjMAXUITEXT] = "qpos";
-  int index = 0;
-
-  // physics: need sync
-  int disable[mjNDISABLE] = {0};
-  int enable[mjNENABLE] = {0};
-  int enableactuator[mjNGROUP] = {0};
-
-  // rendering: need sync
-  int camera = 0;
-
-  // abstract visualization
-  mjvScene scn;
-  mjvCamera& cam;
-  mjvOption& opt;
-  mjvPerturb& pert;
-  mjvFigure figconstraint = {};
-  mjvFigure figcost = {};
-  mjvFigure figtimer = {};
-  mjvFigure figsize = {};
-  mjvFigure figsensor = {};
-
-  // additional user-defined visualization
-  mjvScene* user_scn = nullptr;
-  mjtByte user_scn_flags_prev_[mjNRNDFLAG];
-  std::vector<std::pair<mjrRect, mjvFigure>> user_figures_;
-  std::vector<std::pair<mjrRect, mjvFigure>> user_figures_new_;
-  std::vector<std::tuple<int, int, std::string, std::string>> user_texts_;
-  std::vector<std::tuple<int, int, std::string, std::string>> user_texts_new_;
-  std::vector<std::tuple<mjrRect, std::unique_ptr<unsigned char[]>>> user_images_;
-  std::vector<std::tuple<mjrRect, std::unique_ptr<unsigned char[]>>> user_images_new_;
-
-  // OpenGL rendering and UI
-  int refresh_rate = 60;
-  int window_pos[2] = {0};
-  int window_size[2] = {0};
-  std::unique_ptr<PlatformUIAdapter> platform_ui;
-  mjuiState& uistate;
-  mjUI ui0 = {};
-  mjUI ui1 = {};
-
-  // Constant arrays needed for the option section of UI and the UI interface
-  // TODO setting the size here is not ideal
-  const mjuiDef def_option[13] = {
-    {mjITEM_SECTION,  "Option",        mjPRESERVE, nullptr,  "AO"},
-    {mjITEM_CHECKINT, "Help",          2, &this->help,       " #290"},
-    {mjITEM_CHECKINT, "Info",          2, &this->info,       " #291"},
-    {mjITEM_CHECKINT, "Profiler",      2, &this->profiler,   " #292"},
-    {mjITEM_CHECKINT, "Sensor",        2, &this->sensor,     " #293"},
-    {mjITEM_CHECKINT, "Pause update",  2, &this->pause_update,    ""},
-  #ifdef __APPLE__
-    {mjITEM_CHECKINT, "Fullscreen",    0, &this->fullscreen, " #294"},
-  #else
-    {mjITEM_CHECKINT, "Fullscreen",    1, &this->fullscreen, " #294"},
-  #endif
-    {mjITEM_CHECKINT, "Vertical Sync", 1, &this->vsync,      ""},
-    {mjITEM_CHECKINT, "Busy Wait",     1, &this->busywait,   ""},
-    {mjITEM_SELECT,   "Spacing",       1, &this->spacing,    "Tight\nWide"},
-    {mjITEM_SELECT,   "Color",         1, &this->color,      "Default\nOrange\nWhite\nBlack"},
-    {mjITEM_SELECT,   "Font",          1, &this->font,       "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"},
-    {mjITEM_END}
-  };
-
-
-  // simulation section of UI
-  const mjuiDef def_simulation[14] = {
-    {mjITEM_SECTION,   "Simulation",    mjPRESERVE, nullptr,     "AS"},
-    {mjITEM_RADIO,     "",              5, &this->run,           "Pause\nRun"},
-    {mjITEM_BUTTON,    "Reset",         2, nullptr,              " #259"},
-    {mjITEM_BUTTON,    "Reload",        5, nullptr,              "CL"},
-    {mjITEM_BUTTON,    "Align",         2, nullptr,              "CA"},
-    {mjITEM_BUTTON,    "Copy state",    2, nullptr,              "CC"},
-    {mjITEM_SLIDERINT, "Key",           3, &this->key,           "0 0"},
-    {mjITEM_BUTTON,    "Load key",      3},
-    {mjITEM_BUTTON,    "Save key",      3},
-    {mjITEM_SLIDERNUM, "Noise scale",   5, &this->ctrl_noise_std,  "0 1"},
-    {mjITEM_SLIDERNUM, "Noise rate",    5, &this->ctrl_noise_rate, "0 4"},
-    {mjITEM_SEPARATOR, "History",       1},
-    {mjITEM_SLIDERINT, "",              5, &this->scrub_index,     "0 0"},
-    {mjITEM_END}
-  };
-
-
-  // watch section of UI
-  const mjuiDef def_watch[5] = {
-    {mjITEM_SECTION,   "Watch",         mjPRESERVE, nullptr,     "AW"},
-    {mjITEM_EDITTXT,   "Field",         2, this->field,          "qpos"},
-    {mjITEM_EDITINT,   "Index",         2, &this->index,         "1"},
-    {mjITEM_STATIC,    "Value",         2, nullptr,              " "},
-    {mjITEM_END}
-  };
-
-  // info strings
-  char info_title[Simulate::kMaxFilenameLength] = {0};
-  char info_content[Simulate::kMaxFilenameLength] = {0};
-
-  // pending uploads
-  std::condition_variable_any cond_upload_;
-  int texture_upload_ = -1;
-  int mesh_upload_ = -1;
-  int hfield_upload_ = -1;
-};
-}  // namespace mujoco
-
-#endif

cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_adapter.cc

@@ -1,252 +0,0 @@
-// Copyright 2023 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-
-#include "simulate/mujoco/mujoco_viewer/include/glfw_adapter.h"
-#include <cstdlib>
-#include <utility>
-
-#include <GLFW/glfw3.h>
-#include <mujoco/mjui.h>
-#include <mujoco/mujoco.h>
-#include "simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h"
-
-#ifdef __APPLE__
-#include "glfw_corevideo.h"
-#endif
-
-namespace mujoco {
-namespace {
-int MaybeGlfwInit() {
-  static const int is_initialized = []() {
-    auto success = Glfw().glfwInit();
-    if (success == GLFW_TRUE) {
-      std::atexit(Glfw().glfwTerminate);
-    }
-    return success;
-  }();
-  return is_initialized;
-}
-
-GlfwAdapter& GlfwAdapterFromWindow(GLFWwindow* window) {
-  return *static_cast<GlfwAdapter*>(Glfw().glfwGetWindowUserPointer(window));
-}
-}  // namespace
-
-GlfwAdapter::GlfwAdapter() {
-  if (MaybeGlfwInit() != GLFW_TRUE) {
-    mju_error("could not initialize GLFW");
-  }
-
-  // multisampling
-  Glfw().glfwWindowHint(GLFW_SAMPLES, 4);
-  Glfw().glfwWindowHint(GLFW_VISIBLE, 1);
-
-  // get video mode and save
-  vidmode_ = *Glfw().glfwGetVideoMode(Glfw().glfwGetPrimaryMonitor());
-
-  // create window
-  window_ = Glfw().glfwCreateWindow((2 * vidmode_.width) / 3,
-                                    (2 * vidmode_.height) / 3,
-                                    "MuJoCo", nullptr, nullptr);
-  if (!window_) {
-    mju_error("could not create window");
-  }
-
-  // save window position and size
-  Glfw().glfwGetWindowPos(window_, &window_pos_.first, &window_pos_.second);
-  Glfw().glfwGetWindowSize(window_, &window_size_.first, &window_size_.second);
-
-  // set callbacks
-  Glfw().glfwSetWindowUserPointer(window_, this);
-  Glfw().glfwSetDropCallback(
-      window_, +[](GLFWwindow* window, int count, const char** paths) {
-        GlfwAdapterFromWindow(window).OnFilesDrop(count, paths);
-      });
-  Glfw().glfwSetKeyCallback(
-      window_, +[](GLFWwindow* window, int key, int scancode, int act, int mods) {
-        GlfwAdapterFromWindow(window).OnKey(key, scancode, act);
-      });
-  Glfw().glfwSetMouseButtonCallback(
-      window_, +[](GLFWwindow* window, int button, int act, int mods) {
-        GlfwAdapterFromWindow(window).OnMouseButton(button, act);
-      });
-  Glfw().glfwSetCursorPosCallback(
-      window_, +[](GLFWwindow* window, double x, double y) {
-        GlfwAdapterFromWindow(window).OnMouseMove(x, y);
-      });
-  Glfw().glfwSetScrollCallback(
-      window_, +[](GLFWwindow* window, double xoffset, double yoffset) {
-        GlfwAdapterFromWindow(window).OnScroll(xoffset, yoffset);
-      });
-  Glfw().glfwSetWindowRefreshCallback(
-      window_, +[](GLFWwindow* window) {
-#ifdef __APPLE__
-        auto& core_video = GlfwAdapterFromWindow(window).core_video_;
-        if (core_video.has_value()) {
-          core_video->UpdateDisplayLink();
-        }
-#endif
-        GlfwAdapterFromWindow(window).OnWindowRefresh();
-      });
-  Glfw().glfwSetWindowSizeCallback(
-      window_, +[](GLFWwindow* window, int width, int height) {
-        GlfwAdapterFromWindow(window).OnWindowResize(width, height);
-      });
-
-  // make context current
-  Glfw().glfwMakeContextCurrent(window_);
-}
-
-GlfwAdapter::~GlfwAdapter() {
-  FreeMjrContext();
-  Glfw().glfwMakeContextCurrent(nullptr);
-  Glfw().glfwDestroyWindow(window_);
-}
-
-std::pair<double, double> GlfwAdapter::GetCursorPosition() const {
-  double x, y;
-  Glfw().glfwGetCursorPos(window_, &x, &y);
-  return {x, y};
-}
-
-double GlfwAdapter::GetDisplayPixelsPerInch() const {
-  int width_mm, height_mm;
-  Glfw().glfwGetMonitorPhysicalSize(
-      Glfw().glfwGetPrimaryMonitor(), &width_mm, &height_mm);
-  return 25.4 * vidmode_.width / width_mm;
-}
-
-std::pair<int, int> GlfwAdapter::GetFramebufferSize() const {
-  int width, height;
-  Glfw().glfwGetFramebufferSize(window_, &width, &height);
-  return {width, height};
-}
-
-std::pair<int, int> GlfwAdapter::GetWindowSize() const {
-  int width, height;
-  Glfw().glfwGetWindowSize(window_, &width, &height);
-  return {width, height};
-}
-
-bool GlfwAdapter::IsGPUAccelerated() const {
-  return true;
-}
-
-void GlfwAdapter::PollEvents() {
-  Glfw().glfwPollEvents();
-}
-
-void GlfwAdapter::SetClipboardString(const char* text) {
-  Glfw().glfwSetClipboardString(window_, text);
-}
-
-void GlfwAdapter::SetVSync(bool enabled){
-#ifdef __APPLE__
-  Glfw().glfwSwapInterval(0);
-  if (enabled && !core_video_.has_value()) {
-    core_video_.emplace(window_);
-  } else if (!enabled && core_video_.has_value()) {
-    core_video_.reset();
-  }
-#else
-  Glfw().glfwSwapInterval(enabled);
-#endif
-}
-
-void GlfwAdapter::SetWindowTitle(const char* title) {
-  Glfw().glfwSetWindowTitle(window_, title);
-}
-
-bool GlfwAdapter::ShouldCloseWindow() const {
-  return Glfw().glfwWindowShouldClose(window_);
-}
-
-void GlfwAdapter::SwapBuffers() {
-#ifdef __APPLE__
-  if (core_video_.has_value()) {
-    core_video_->WaitForDisplayRefresh();
-  }
-#endif
-  Glfw().glfwSwapBuffers(window_);
-}
-
-void GlfwAdapter::ToggleFullscreen() {
-  // currently full screen: switch to windowed
-  if (Glfw().glfwGetWindowMonitor(window_)) {
-    // restore window from saved data
-    Glfw().glfwSetWindowMonitor(window_, nullptr, window_pos_.first, window_pos_.second,
-                                window_size_.first, window_size_.second, 0);
-  }
-
-  // currently windowed: switch to full screen
-  else {
-    // save window data
-    Glfw().glfwGetWindowPos(window_, &window_pos_.first, &window_pos_.second);
-    Glfw().glfwGetWindowSize(window_, &window_size_.first,
-                             &window_size_.second);
-
-    // switch
-    Glfw().glfwSetWindowMonitor(window_, Glfw().glfwGetPrimaryMonitor(), 0,
-                                0, vidmode_.width, vidmode_.height,
-                                vidmode_.refreshRate);
-  }
-}
-
-bool GlfwAdapter::IsLeftMouseButtonPressed() const {
-  return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsMiddleMouseButtonPressed() const {
-  return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsRightMouseButtonPressed() const {
-  return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsAltKeyPressed() const {
-  return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_ALT) == GLFW_PRESS ||
-         Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_ALT) == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsCtrlKeyPressed() const {
-  return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS ||
-         Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsShiftKeyPressed() const {
-  return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS ||
-         Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsMouseButtonDownEvent(int act) const {
-  return act == GLFW_PRESS;
-}
-
-bool GlfwAdapter::IsKeyDownEvent(int act) const { return act == GLFW_PRESS; }
-
-int GlfwAdapter::TranslateKeyCode(int key) const { return key; }
-
-mjtButton GlfwAdapter::TranslateMouseButton(int button) const {
-  if (button == GLFW_MOUSE_BUTTON_LEFT) {
-    return mjBUTTON_LEFT;
-  } else if (button == GLFW_MOUSE_BUTTON_RIGHT) {
-    return mjBUTTON_RIGHT;
-  } else if (button == GLFW_MOUSE_BUTTON_MIDDLE) {
-    return mjBUTTON_MIDDLE;
-  }
-  return mjBUTTON_NONE;
-}
-}  // namespace mujoco

cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_dispatch.cc

@@ -1,127 +0,0 @@
-// Copyright 2022 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h"
-
-#ifdef mjGLFW_DYNAMIC_SYMBOLS
-  #ifdef _MSC_VER
-    #include <windows.h>
-    #include <libloaderapi.h>
-  #else
-    #include <dlfcn.h>
-  #endif
-#endif
-
-#include <cstdlib>
-#include <iostream>
-
-namespace mujoco {
-
-// return dispatch table for glfw functions
-const struct Glfw& Glfw(void* dlhandle) {
-  {
-    // set static init_dlhandle
-    static const void* init_dlhandle = dlhandle;
-
-    // check that not already initialized
-    if (dlhandle && dlhandle != init_dlhandle) {
-      std::cerr << "dlhandle is specified when GLFW dispatch table is already "
-                   "initialized\n";
-      abort();
-    }
-  }
-
-  // make and intialize dispatch table
-  static const struct Glfw glfw = [&]() {  // create and call constructor
-    // allocate
-    struct Glfw glfw;
-
-    // load glfw dynamically
-#ifdef mjGLFW_DYNAMIC_SYMBOLS
-  #ifdef _MSC_VER
-    if (!dlhandle) dlhandle = LoadLibraryA("glfw3.dll");
-    if (!dlhandle) {
-      std::cerr << "cannot obtain a shared object handle\n";
-      abort();
-    }
-    #define mjGLFW_RESOLVE_SYMBOL(func)                   \
-      glfw.func = reinterpret_cast<decltype(glfw.func)>(  \
-          GetProcAddress(reinterpret_cast<HMODULE>(dlhandle), #func))
-  #else
-    if (!dlhandle) dlhandle = dlopen("nullptr", RTLD_GLOBAL | RTLD_NOW);
-    if (!dlhandle) {
-      std::cerr << "cannot obtain a shared object handle\n";
-      abort();
-    }
-    #define mjGLFW_RESOLVE_SYMBOL(func) \
-      glfw.func = reinterpret_cast<decltype(glfw.func)>(dlsym(dlhandle, #func))
-  #endif
-#else
-  #define mjGLFW_RESOLVE_SYMBOL(func) glfw.func = &::func
-#endif
-
-    // set pointers in dispatch table
-#define mjGLFW_INITIALIZE_SYMBOL(func)                  \
-    if (!(mjGLFW_RESOLVE_SYMBOL(func))) {               \
-      std::cerr << "cannot dlsym " #func "\n";          \
-      abort();                                          \
-    }
-
-    // go/keep-sorted start
-    mjGLFW_INITIALIZE_SYMBOL(glfwCreateWindow);
-    mjGLFW_INITIALIZE_SYMBOL(glfwDestroyWindow);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetCursorPos);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetFramebufferSize);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetKey);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetMonitorPhysicalSize);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetMouseButton);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetPrimaryMonitor);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetTime);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetVideoMode);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowMonitor);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowPos);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowSize);
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowUserPointer);
-    mjGLFW_INITIALIZE_SYMBOL(glfwInit);
-    mjGLFW_INITIALIZE_SYMBOL(glfwMakeContextCurrent);
-    mjGLFW_INITIALIZE_SYMBOL(glfwPollEvents);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetClipboardString);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetCursorPosCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetDropCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetKeyCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetMouseButtonCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetScrollCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowMonitor);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowRefreshCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowSizeCallback);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowTitle);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowUserPointer);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSwapBuffers);
-    mjGLFW_INITIALIZE_SYMBOL(glfwSwapInterval);
-    mjGLFW_INITIALIZE_SYMBOL(glfwTerminate);
-    mjGLFW_INITIALIZE_SYMBOL(glfwWindowHint);
-    mjGLFW_INITIALIZE_SYMBOL(glfwWindowShouldClose);
-    // go/keep-sorted end
-
-#ifdef __APPLE__
-    mjGLFW_INITIALIZE_SYMBOL(glfwGetNSGLContext);
-#endif
-
-#undef mjGLFW_INITIALIZE_SYMBOL
-
-    return glfw;
-  }();
-  return glfw;
-}
-}  // namespace mujoco

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp

@@ -4,22 +4,20 @@
 
 
 #include <algorithm>
-#include <cstdio>
 #include <chrono>
 #include <cmath>
+#include <cstdio>
 #include <limits>
+#include <thread>
+#include <utility>
+
+#include "common/base/logging/logger.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
-#include "simulate/mujoco/mujoco_viewer/include/array_safety.h"
+#include <simulate/glfw_adapter.h>
-#include "simulate/mujoco/mujoco_viewer/include/glfw_adapter.h"
 
 namespace cmvr {
     namespace mj = ::mujoco;
-    namespace mju = ::mujoco::sample_util;
 
-    using Seconds = std::chrono::duration<double>;
-
-    constexpr double kSyncMisalign = 0.1;
-    constexpr double kSimRefreshFraction = 0.7;
     constexpr int kPiPMaxGeom = 100000;
 
     class PiPGlfwAdapter : public mj::GlfwAdapter {
@@ -38,55 +36,15 @@ namespace cmvr {
         MuJocoViewer *owner_;
     };
 
-
+    MuJocoViewer::MuJocoViewer(std::shared_ptr<simulate::MujocoWorld> world)
-    static const char *CheckDiverged(int disableflags, const mjData *d) {
+        : world_(std::move(world)) {
-        if (disableflags & mjDSBL_AUTORESET) {
-            for (mjtWarning w: {mjWARN_BADQACC, mjWARN_BADQVEL, mjWARN_BADQPOS}) {
-                if (d->warning[w].number > 0) {
-                    return mju_warningText(w, d->warning[w].lastinfo);
-                }
-            }
-        }
-        return nullptr;
-    }
-
-
-    mjModel *MuJocoViewer::LoadModelSimple(const char *file,
-                                           char *load_error,
-                                           int error_sz) {
-        load_error[0] = '\0';
-        if (!file || !file[0]) {
-            std::snprintf(load_error, error_sz, "empty model filename");
-            return nullptr;
-        }
-
-        mjModel *mnew = nullptr;
-        const char *dot = std::strrchr(file, '.');
-
-        if (dot && std::strcmp(dot, ".mjb") == 0) {
-            mnew = mj_loadModel(file, nullptr);
-            if (!mnew) {
-                std::snprintf(load_error, error_sz,
-                              "could not load binary model: %s", file);
-            }
-        } else {
-            mnew = mj_loadXML(file, nullptr, load_error, error_sz);
-            if (load_error[0]) {
-                int len = std::strlen(load_error);
-                if (len > 0 && load_error[len - 1] == '\n') {
-                    load_error[len - 1] = '\0';
-                }
-            }
-        }
-        return mnew;
-    }
-
-    MuJocoViewer::MuJocoViewer(const char *model_path)
-        : model_path_(model_path ? model_path : "") {
         std::printf("MuJoCo version %s\n", mj_versionString());
         if (mjVERSION_HEADER != mj_version()) {
             mju_error("Headers and library have different versions");
         }
+        if (!world_) {
+            mju_error("MuJocoViewer requires a MujocoWorld");
+        }
 
         initSim();
     }
@@ -102,7 +60,7 @@ namespace cmvr {
         sim_ = std::make_unique<mj::Simulate>(
             std::move(platform_ui),
             &cam_, &opt_, &pert_,
-            /*is_passive=*/false
+            /*is_passive=*/true
         );
     }
 
@@ -110,47 +68,22 @@ namespace cmvr {
         if (sim_) {
             sim_->exitrequest.store(1);
         }
-        if (physics_started_.load() && physics_thread_.joinable()) {
+        if (sync_thread_.joinable()) {
-            physics_thread_.join();
+            sync_thread_.join();
         }
 
         if (pip_scene_inited_) {
             mjv_freeScene(&pip_scene_);
             pip_scene_inited_ = false;
         }
-
-        if (d_) mj_deleteData(d_);
-        if (m_) mj_deleteModel(m_);
     }
 
-    void MuJocoViewer::physicsThreadFunc() {
+    mjModel* MuJocoViewer::model() const {
-        // 1) 加载模型
+        return world_ ? world_->model() : nullptr;
-        char error[1024];
+    }
-        m_ = LoadModelSimple(model_path_.c_str(), error, sizeof(error));
-        if (!m_) {
-            mju_error("Load model failed: %s", error);
-        }
 
-        d_ = mj_makeData(m_);
+    mjData* MuJocoViewer::data() const {
-        if (!d_) {
+        return world_ ? world_->data() : nullptr;
-            mj_deleteModel(m_);
-            m_ = nullptr;
-            mju_error("mj_makeData failed");
-        }
-
-        // 2) 用官方的方式：先告诉 Simulate 要加载，然后等 RenderLoop 处理
-        sim_->Load(m_, d_, model_path_.c_str());
-
-        {
-            // official main 里也是在 Load 之后先 forward 一下
-            const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
-            mj_forward(m_, d_);
-        }
-
-        physics_started_.store(true);
-
-        // 3) 进入物理循环
-        physicsLoop();
     }
 
     void MuJocoViewer::getCameraState(double &distance,
@@ -165,12 +98,12 @@ namespace cmvr {
                                    double azimuth,
                                    double elevation) {
         // 相机初始化：根据模型设置一个合理的 free camera
-        if (m_) {
+        if (model()) {
-            mjv_defaultFreeCamera(m_, &cam_);
+            mjv_defaultFreeCamera(model(), &cam_);
             // 以模型中心为观察点
-            cam_.lookat[0] = m_->stat.center[0];
+            cam_.lookat[0] = model()->stat.center[0];
-            cam_.lookat[1] = m_->stat.center[1];
+            cam_.lookat[1] = model()->stat.center[1];
-            cam_.lookat[2] = m_->stat.center[2];
+            cam_.lookat[2] = model()->stat.center[2];
         } else {
             mjv_defaultCamera(&cam_);
         }
@@ -187,6 +120,8 @@ namespace cmvr {
         pip_camera_id_ = -1;
         pip_width_ = 320;
         pip_height_ = 240;
+        pip_render_width_ = pip_width_;
+        pip_render_height_ = pip_height_;
         pip_custom_pos_ = false;
     }
 
@@ -195,13 +130,25 @@ namespace cmvr {
                                        int bottom,
                                        int width,
                                        int height) {
+        enablePiPCamera(camera_name, left, bottom, width, height, width, height);
+    }
+
+    void MuJocoViewer::enablePiPCamera(const char *camera_name,
+                                       int left,
+                                       int bottom,
+                                       int display_width,
+                                       int display_height,
+                                       int render_width,
+                                       int render_height) {
         pip_enabled_ = true;
         pip_camera_name_ = camera_name ? camera_name : "";
         pip_camera_id_ = -1;
         pip_left_ = left;
         pip_bottom_ = bottom;
-        pip_width_ = width > 0 ? width : 320;
+        pip_width_ = display_width > 0 ? display_width : 320;
-        pip_height_ = height > 0 ? height : 240;
+        pip_height_ = display_height > 0 ? display_height : 240;
+        pip_render_width_ = render_width > 0 ? render_width : pip_width_;
+        pip_render_height_ = render_height > 0 ? render_height : pip_height_;
         pip_custom_pos_ = true;
     }
 
@@ -212,11 +159,15 @@ namespace cmvr {
 
 
     void MuJocoViewer::renderPiP() {
-        if (!pip_enabled_ || !sim_ || !m_ || !d_) return;
+        if (!pip_enabled_ || !sim_) return;
         if (pip_camera_name_.empty()) return;
 
+        mjModel* render_model = sim_->m_passive_ ? sim_->m_passive_ : sim_->m_;
+        mjData* render_data = sim_->d_passive_ ? sim_->d_passive_ : sim_->d_;
+        if (!render_model || !render_data) return;
+
         if (pip_camera_id_ < 0) {
-            pip_camera_id_ = mj_name2id(m_, mjOBJ_CAMERA, pip_camera_name_.c_str());
+            pip_camera_id_ = mj_name2id(render_model, mjOBJ_CAMERA, pip_camera_name_.c_str());
             if (pip_camera_id_ < 0) {
                 return;
             }
@@ -231,10 +182,16 @@ namespace cmvr {
         int height = 0;
 
         if (pip_custom_pos_) {
-            left = std::max(0, std::min(pip_left_, fb_width - 1));
+            width = std::min(pip_width_, fb_width);
-            bottom = std::max(0, std::min(pip_bottom_, fb_height - 1));
+            height = std::min(pip_height_, fb_height);
-            width = std::min(pip_width_, fb_width - left);
+            const int max_left = fb_width - width;
-            height = std::min(pip_height_, fb_height - bottom);
+            const int max_bottom = fb_height - height;
+            left = pip_left_ >= 0
+                       ? std::max(0, std::min(pip_left_, max_left))
+                       : std::max(0, std::min(fb_width - width + pip_left_, max_left));
+            bottom = pip_bottom_ >= 0
+                         ? std::max(0, std::min(pip_bottom_, max_bottom))
+                         : std::max(0, std::min(fb_height - height + pip_bottom_, max_bottom));
         } else {
             width = std::min(pip_width_, fb_width - 2 * pip_margin_);
             height = std::min(pip_height_, fb_height - 2 * pip_margin_);
@@ -244,41 +201,80 @@ namespace cmvr {
 
         if (width <= 0 || height <= 0) return;
 
-        mjrRect rect;
+        mjrRect display_rect;
-        rect.width = width;
+        display_rect.width = width;
-        rect.height = height;
+        display_rect.height = height;
-        rect.left = left;
+        display_rect.left = left;
-        rect.bottom = bottom;
+        display_rect.bottom = bottom;
 
         const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
 
-        if (!pip_scene_inited_ || pip_scene_model_ != m_) {
+        if (!pip_scene_inited_ || pip_scene_model_ != render_model) {
             if (pip_scene_inited_) {
                 mjv_freeScene(&pip_scene_);
             }
-            mjv_makeScene(m_, &pip_scene_, kPiPMaxGeom);
+            mjv_makeScene(render_model, &pip_scene_, kPiPMaxGeom);
             pip_scene_inited_ = true;
-            pip_scene_model_ = m_;
+            pip_scene_model_ = render_model;
         }
 
         pip_cam_.type = mjCAMERA_FIXED;
         pip_cam_.fixedcamid = pip_camera_id_;
         pip_cam_.trackbodyid = -1;
 
-        mjv_updateScene(m_, d_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
+        mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
-        mjr_render(rect, &pip_scene_, &sim_->platform_ui->mjr_context());
+
+        auto& context = sim_->platform_ui->mjr_context();
+        const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
+        const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
+        const int render_width = std::min(std::max(1, pip_render_width_), offscreen_width);
+        const int render_height = std::min(std::max(1, pip_render_height_), offscreen_height);
+        if (!pip_render_size_warning_logged_ &&
+            (render_width != pip_render_width_ || render_height != pip_render_height_)) {
+            CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
+                              << ", requested=" << pip_render_width_ << "x" << pip_render_height_
+                              << ", actual=" << render_width << "x" << render_height
+                              << ", offscreen=" << offscreen_width << "x" << offscreen_height;
+            pip_render_size_warning_logged_ = true;
+        }
+        const bool use_offscreen = render_width != display_rect.width || render_height != display_rect.height;
+
+        mjrRect render_rect;
+        render_rect.left = 0;
+        render_rect.bottom = 0;
+        render_rect.width = render_width;
+        render_rect.height = render_height;
+
+        bool rendered_offscreen = false;
+        if (use_offscreen) {
+            mjr_setBuffer(mjFB_OFFSCREEN, &context);
+            if (context.currentBuffer == mjFB_OFFSCREEN) {
+                mjr_render(render_rect, &pip_scene_, &context);
+                rendered_offscreen = true;
+            } else {
+                mjr_render(display_rect, &pip_scene_, &context);
+                render_rect = display_rect;
+            }
+        } else {
+            mjr_render(display_rect, &pip_scene_, &context);
+            render_rect = display_rect;
+        }
 
         {
             std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
-            const int w = rect.width;
+            const int w = render_rect.width;
-            const int h = rect.height;
+            const int h = render_rect.height;
             if (w > 0 && h > 0) {
                 pip_rgb_.resize(static_cast<size_t>(3 * w * h));
                 pip_depth_.resize(static_cast<size_t>(w * h));
 
                 // 同时读 RGB 和 depth(z-buffer 0..1)
                 mjr_readPixels(pip_rgb_.data(), pip_depth_.data(),
-                               rect, &sim_->platform_ui->mjr_context());
+                               render_rect, &context);
+                if (rendered_offscreen) {
+                    mjr_setBuffer(mjFB_WINDOW, &context);
+                    mjr_render(display_rect, &pip_scene_, &context);
+                }
 
                 // OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth
                 for (int r = 0; r < h / 2; ++r) {
@@ -294,8 +290,10 @@ namespace cmvr {
                 }
 
                 // 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。
-                const double znear = static_cast<double>(m_->vis.map.znear) * static_cast<double>(m_->stat.extent);
+                const double znear = static_cast<double>(render_model->vis.map.znear) *
-                const double zfar  = static_cast<double>(m_->vis.map.zfar)  * static_cast<double>(m_->stat.extent);
+                                     static_cast<double>(render_model->stat.extent);
+                const double zfar  = static_cast<double>(render_model->vis.map.zfar) *
+                                     static_cast<double>(render_model->stat.extent);
                 if (znear > 0.0 && zfar > znear) {
                     const double two_nf = 2.0 * znear * zfar;
                     const double f_plus_n = zfar + znear;
@@ -323,137 +321,51 @@ namespace cmvr {
         }
     }
 
-    void MuJocoViewer::physicsLoop() {
+    void MuJocoViewer::syncThreadFunc() {
-        using Clock = mj::Simulate::Clock;
+        if (!world_->isLoaded()) {
+            mju_error("MuJocoViewer requires a loaded MujocoWorld");
+        }
 
-        std::chrono::time_point<Clock> syncCPU;
+        {
-        mjtNum syncSim = 0;
+            std::lock_guard<std::mutex> lock(world_->mutex());
+            if (!world_->model() || !world_->data()) {
+                mju_error("MuJocoViewer world has null model/data");
+            }
+            if (pip_enabled_ && pip_render_width_ > 0 && pip_render_height_ > 0) {
+                mjModel* model = world_->model();
+                const int old_width = model->vis.global.offwidth;
+                const int old_height = model->vis.global.offheight;
+                model->vis.global.offwidth = std::max(model->vis.global.offwidth, pip_render_width_);
+                model->vis.global.offheight = std::max(model->vis.global.offheight, pip_render_height_);
+                if (model->vis.global.offwidth != old_width || model->vis.global.offheight != old_height) {
+                    CMVR_LOG(INFO) << "[MuJocoViewer] resize offscreen buffer before context creation"
+                                   << ", old=" << old_width << "x" << old_height
+                                   << ", new=" << model->vis.global.offwidth << "x" << model->vis.global.offheight;
+                }
+            }
+            sim_->Load(world_->model(), world_->data(), world_->modelPath().c_str());
+        }
 
         while (!sim_->exitrequest.load()) {
-            // 给 UI 线程一点时间
-            if (sim_->run && sim_->busywait) {
-                std::this_thread::yield();
-            } else {
-                std::this_thread::sleep_for(std::chrono::milliseconds(1));
-            }
-
             {
-                const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
+                std::lock_guard<std::mutex> lock(world_->mutex());
-                if (!m_ || !d_) continue;
+                sim_->Sync(/*state_only=*/true);
-
-                if (sim_->run) {
-                    bool stepped = false;
-                    const auto startCPU = Clock::now();
-                    const auto elapsedCPU = startCPU - syncCPU;
-                    double elapsedSim = d_->time - syncSim;
-
-                    double slowdown = 100.0 / sim_->percentRealTime[sim_->real_time_index];
-                    bool misaligned =
-                            std::abs(Seconds(elapsedCPU).count() / slowdown - elapsedSim) > kSyncMisalign;
-
-                    if (elapsedSim < 0 || elapsedCPU.count() < 0 ||
-                        syncCPU.time_since_epoch().count() == 0 ||
-                        misaligned || sim_->speed_changed) {
-                        syncCPU = startCPU;
-                        syncSim = d_->time;
-                        sim_->speed_changed = false;
-
-                        sim_->InjectNoise(sim_->key);
-
-
-                        controlDispatch(m_, d_);
-
-                        mj_step(m_, d_);
-                        const char *msg = CheckDiverged(m_->opt.disableflags, d_);
-                        if (msg) {
-                            sim_->run = 0;
-                            mju::strcpy_arr(sim_->load_error, msg);
-                        } else {
-                            stepped = true;
-                        }
-                    } else {
-                        bool measured = false;
-                        mjtNum prevSim = d_->time;
-                        double refreshTime = kSimRefreshFraction / sim_->refresh_rate;
-
-                        while (Seconds((d_->time - syncSim) * slowdown) <
-                               (Clock::now() - syncCPU) &&
-                               (Clock::now() - startCPU) < Seconds(refreshTime)) {
-                            if (!measured && elapsedSim) {
-                                sim_->measured_slowdown =
-                                        Seconds(elapsedCPU).count() / elapsedSim;
-                                measured = true;
-                            }
-
-                            sim_->InjectNoise(sim_->key);
-
-
-                            controlDispatch(m_, d_);
-
-                            mj_step(m_, d_);
-                            const char *msg = CheckDiverged(m_->opt.disableflags, d_);
-                            if (msg) {
-                                sim_->run = 0;
-                                mju::strcpy_arr(sim_->load_error, msg);
-                            } else {
-                                stepped = true;
-                            }
-
-                            if (d_->time < prevSim) {
-                                break;
-                            }
-                        }
-                    }
-
-                    if (stepped) {
-                        sim_->AddToHistory();
-                    }
-                } else {
-                    mj_forward(m_, d_);
-                    if (sim_->pause_update) {
-                        mju_copy(d_->qacc_warmstart, d_->qacc, m_->nv);
-                    }
-                    sim_->speed_changed = true;
-                }
-
-                if (d_->time == 0.0) {
-                    onReset(m_, d_);
-                }
             }
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
     }
 
-    void MuJocoViewer::controlDispatch(mjModel *m, mjData *d) {
-        // 第一次：只跑一次 initOnce
-        if (!inited_) {
-            initOnce(m, d);
-            inited_ = true;
-            last_time_ = d->time;
-        }
-
-        // 检测 reset：时间从大跳到小（比如从 3.2 回到 0）
-        if (d->time < last_time_) {
-            onReset(m, d);
-            inited_ = false;
-        }
-        last_time_ = d->time;
-
-        // 每步控制
-        controlCallback(m, d);
-    }
-
     void MuJocoViewer::run() {
-        // 先起 physics 线程（里面会 sim_->Load）
+        sync_thread_ = std::thread(&MuJocoViewer::syncThreadFunc, this);
-        physics_thread_ = std::thread(&MuJocoViewer::physicsThreadFunc, this);
 
         // 当前线程跑 UI + 渲染（阻塞）
         sim_->RenderLoop();
 
-        // UI 退出后，通知 physics 线程结束
+        // UI 退出后，通知同步线程结束
         sim_->exitrequest.store(2);
 
-        if (physics_thread_.joinable()) {
+        if (sync_thread_.joinable()) {
-            physics_thread_.join();
+            sync_thread_.join();
         }
     }
 
@@ -492,4 +404,180 @@ namespace cmvr {
         frame_id = pip_frame_id_;
         return true;
     }
+
+    MujocoViewerDevice::MujocoViewerDevice(config::MujocoViewerConfig config)
+        : device::AbstractDevice(config.id()), config_(std::move(config)) {}
+
+    MujocoViewerDevice::~MujocoViewerDevice() {
+        stop();
+    }
+
+    bool MujocoViewerDevice::init() {
+        std::lock_guard<std::mutex> lock(mtx_);
+        if (world_) {
+            return true;
+        }
+        if (config_.id().empty()) {
+            CMVR_LOG(ERROR) << "[MujocoViewerDevice] id is empty";
+            return false;
+        }
+        if (config_.world_id().empty()) {
+            CMVR_LOG(ERROR) << "[MujocoViewerDevice] world_id is empty, id=" << id_;
+            return false;
+        }
+
+        auto world = simulate::MujocoWorldDevice::worldFor(config_.world_id());
+        if (!world) {
+            CMVR_LOG(ERROR) << "[MujocoViewerDevice] MuJoCo world not found: "
+                            << config_.world_id();
+            return false;
+        }
+        if (!world->isLoaded()) {
+            CMVR_LOG(ERROR) << "[MujocoViewerDevice] MuJoCo world is not loaded: "
+                            << config_.world_id();
+            return false;
+        }
+
+        world_ = std::move(world);
+        CMVR_LOG(INFO) << "[MujocoViewerDevice] initialized, id=" << id_
+                       << ", world_id=" << config_.world_id();
+        return true;
+    }
+
+    bool MujocoViewerDevice::start() {
+        CMVR_LOG(INFO) << "[MujocoViewerDevice] start skipped, id=" << id_
+                       << ", call runOnMainThread() from main thread to show UI";
+        return true;
+    }
+
+    bool MujocoViewerDevice::runOnMainThread() {
+        if (!init()) {
+            return false;
+        }
+
+        std::shared_ptr<simulate::MujocoWorld> world;
+        {
+            std::lock_guard<std::mutex> lock(mtx_);
+            if (running_) {
+                return true;
+            }
+            if (!world_) {
+                CMVR_LOG(ERROR) << "[MujocoViewerDevice] world is null, id=" << id_;
+                return false;
+            }
+            world = world_;
+            stop_requested_ = false;
+            running_ = true;
+        }
+
+        if (!world->isRunning() && !world->start()) {
+            CMVR_LOG(ERROR) << "[MujocoViewerDevice] failed to start world, id=" << id_
+                            << ", error=" << world->lastError();
+            std::lock_guard<std::mutex> lock(mtx_);
+            running_ = false;
+            return false;
+        }
+
+        auto viewer = std::make_unique<MuJocoViewer>(world);
+        const double distance = config_.camera_distance() > 0.0
+            ? config_.camera_distance()
+            : 3.0;
+        viewer->setupCamera(distance,
+                            config_.camera_azimuth(),
+                            config_.camera_elevation());
+
+        if (has_pip_camera_config_ && !pip_camera_config_.camera_name().empty()) {
+            const auto& pip = pip_camera_config_.viewer_pip();
+            const auto& render = pip_camera_config_.render();
+            if (pip.width() > 0 && pip.height() > 0) {
+                viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str(),
+                                        pip.left(),
+                                        pip.bottom(),
+                                        pip.width(),
+                                        pip.height(),
+                                        render.width(),
+                                        render.height());
+            } else {
+                viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str());
+            }
+        }
+
+        {
+            std::lock_guard<std::mutex> lock(mtx_);
+            viewer_ = std::move(viewer);
+        }
+
+        CMVR_LOG(INFO) << "[MujocoViewerDevice] run UI on main thread, id=" << id_;
+        viewer_->run();
+
+        {
+            std::lock_guard<std::mutex> lock(mtx_);
+            viewer_.reset();
+            running_ = false;
+        }
+        return true;
+    }
+
+    bool MujocoViewerDevice::setPiPCameraConfig(const config::MujocoCameraConfig& camera_config) {
+        if (!camera_config.viewer_pip().enable()) {
+            return false;
+        }
+        if (camera_config.world_id() != config_.world_id()) {
+            return false;
+        }
+
+        std::lock_guard<std::mutex> lock(mtx_);
+        if (has_pip_camera_config_) {
+            CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first"
+                              << ", viewer_id=" << id_
+                              << ", current_camera=" << pip_camera_config_.camera_name()
+                              << ", ignored_camera=" << camera_config.camera_name();
+            return false;
+        }
+
+        pip_camera_config_ = camera_config;
+        has_pip_camera_config_ = true;
+        CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera"
+                       << ", viewer_id=" << id_
+                       << ", camera=" << camera_config.camera_name()
+                       << ", world_id=" << camera_config.world_id();
+        return true;
+    }
+
+    bool MujocoViewerDevice::getPiPCameraRGBD(std::vector<unsigned char>& rgb,
+                                              std::vector<float>& depth,
+                                              int& width,
+                                              int& height,
+                                              uint64_t& frame_id) const {
+        std::lock_guard<std::mutex> lock(mtx_);
+        if (!viewer_) {
+            return false;
+        }
+        return viewer_->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+    }
+
+    bool MujocoViewerDevice::stop() {
+        std::thread thread_to_join;
+        {
+            std::lock_guard<std::mutex> lock(mtx_);
+            stop_requested_ = true;
+            if (viewer_) {
+                viewer_->requestStop();
+            }
+            if (viewer_thread_.joinable()) {
+                thread_to_join = std::move(viewer_thread_);
+            }
+        }
+
+        if (thread_to_join.joinable()) {
+            thread_to_join.join();
+        }
+
+        {
+            std::lock_guard<std::mutex> lock(mtx_);
+            running_ = false;
+            viewer_.reset();
+        }
+        return true;
+    }
 } // namespace cmvr

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp

@@ -1,270 +1,65 @@
-//
+#include <cstdlib>
-// Created by lgv on 11/25/25.
+#include <filesystem>
-//
+#include <iostream>
+#include <memory>
+#include <string>
 
-#include "algorithms/controllers/pid/include/pid_controller.h"
+#include <gtest/gtest.h>
 
-#include "gtest/gtest.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
-#include <thread>
+namespace {
-#include <array>
-#include <vector>
-#include <cstdio>
 
-#include "common/vision/matplotlibcpp.h"
+std::filesystem::path findProjectRoot()
-
-
-namespace plt = matplotlibcpp;
-
-
-using namespace cmvr;
-
-class MyViewer : public cmvr::MuJocoViewer {
-public:
-    using MuJocoViewer::MuJocoViewer;
-
-    // 对外接口：设置右臂关节目标角度（rad）
-    void moveJ(const std::array<double, 7>& q_target) {
-        q_target_ = q_target;
-    }
-
-    // 仿真结束后调用：画出 7 个关节的目标值与实际值曲线（7 行子图）
-    void plotJointResponse(const std::string& filename = "right_arm_joint_response.png");
-
-private:
-    int print_counter_ = 0;   // 打印计数器
-
-protected:
-    static constexpr int kRightArmDOF = 7;
-
-    int act_ids_[kRightArmDOF];   // actuator 索引
-    int qpos_ids_[kRightArmDOF];  // qpos 索引
-    int qvel_ids_[kRightArmDOF];  // qvel 索引
-
-    std::array<double, kRightArmDOF> q_target_{};  // 目标关节角
-
-    // PID 控制器
-    PidController pid_;
-    double dt_ = 0.0;   // 仿真时间步长
-
-    // 日志：时间 & 每帧的 7 关节实际角 / 目标角
-    std::vector<double> time_log_;
-    std::vector<std::array<double, kRightArmDOF>> q_log_;
-    std::vector<std::array<double, kRightArmDOF>> q_des_log_;
-
-    void initOnce(mjModel* m, mjData* d) override
-    {
-        // 设置相机视角参数
-        setupCamera(3.0, -170, -41);
-
-        dt_ = m->opt.timestep;
-
-        const char* act_names[kRightArmDOF] = {
-            "R_SHOULDER_P_tau",
-            "R_SHOULDER_R_tau",
-            "R_SHOULDER_Y_tau",
-            "R_ELBOW_R_tau",
-            "R_WRIST_P_tau",
-            "R_WRIST_Y_tau",
-            "R_WRIST_R_tau"
-        };
-
-        const char* jnt_names[kRightArmDOF] = {
-            "R_SHOULDER_P",
-            "R_SHOULDER_R",
-            "R_SHOULDER_Y",
-            "R_ELBOW_R",
-            "R_WRIST_P",
-            "R_WRIST_Y",
-            "R_WRIST_R"
-        };
-
-        for (int i = 0; i < kRightArmDOF; ++i) {
-            // actuator 索引
-            act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
-            if (act_ids_[i] < 0) {
-                std::fprintf(stderr, "Cannot find actuator %s\n", act_names[i]);
-            }
-
-            // joint -> qpos/qvel 索引
-            int jnt_id = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]);
-            if (jnt_id < 0) {
-                std::fprintf(stderr, "Cannot find joint %s\n", jnt_names[i]);
-                qpos_ids_[i] = -1;
-                qvel_ids_[i] = -1;
-            } else {
-                qpos_ids_[i] = m->jnt_qposadr[jnt_id];  // 位置
-                qvel_ids_[i] = m->jnt_dofadr[jnt_id];   // 速度
-            }
-        }
-
-        // === 初始化 PID 控制器 ===
-        // 注意：这里用的是你自己的接口，如果你的是 setDof，就改成 pid_.setDof(kRightArmDOF);
-        pid_.init(kRightArmDOF);
-
-        // 简单一组增益（后面可以按关节单独调）
-        Eigen::VectorXd Kp = Eigen::VectorXd::Constant(kRightArmDOF, 50.0);
-        Eigen::VectorXd Ki = Eigen::VectorXd::Constant(kRightArmDOF, 100);
-        Eigen::VectorXd Kd = Eigen::VectorXd::Constant(kRightArmDOF, 0);
-        pid_.setGains(Kp, Ki, Kd);
-
-        // 积分上下限
-        Eigen::VectorXd iLower = Eigen::VectorXd::Constant(kRightArmDOF, -10.0);
-        Eigen::VectorXd iUpper = Eigen::VectorXd::Constant(kRightArmDOF,  10.0);
-        pid_.setIntegralLimits(iLower, iUpper);
-
-        // 误差死区（小于 0.002 rad 不管）
-        Eigen::VectorXd deadzone = Eigen::VectorXd::Constant(kRightArmDOF, 0.002);
-        pid_.setDeadzone(deadzone);
-
-        // D 项滤波系数（可根据实际调）
-        pid_.setDerivativeFilterCoeff(50.0);
-
-        std::printf("[MyViewer] initOnce done\n");
-    }
-
-    // 每个仿真步物理线程都会调用这里
-    void controlCallback(mjModel* m, mjData* d) override
-    {
-        // === 1. 组装当前状态 ===
-        Eigen::VectorXd q(kRightArmDOF);
-        Eigen::VectorXd dq(kRightArmDOF);
-
-        for (int i = 0; i < kRightArmDOF; ++i) {
-            if (qpos_ids_[i] >= 0)
-                q[i] = d->qpos[qpos_ids_[i]];
-            else
-                q[i] = 0.0;
-
-            if (qvel_ids_[i] >= 0)
-                dq[i] = d->qvel[qvel_ids_[i]];
-            else
-                dq[i] = 0.0;
-        }
-
-        ControlInput input;
-        input.q  = q;
-        input.dq = dq;
-
-        // === 2. 组装参考（目标角度） ===
-        ControlReference ref;
-        ref.q_d  = Eigen::Map<const Eigen::VectorXd>(q_target_.data(), kRightArmDOF);
-        ref.dq_d = Eigen::VectorXd::Zero(kRightArmDOF);
-
-        pid_.setReference(ref);
-
-        // === 3. 计算关节力矩 τ ===
-        Eigen::VectorXd tau = pid_.compute(input, dt_);
-
-        // === 4. 写入 MuJoCo ctrl（motor: ctrl = torque） ===
-        for (int i = 0; i < kRightArmDOF; ++i) {
-            if (act_ids_[i] < 0) continue;
-            d->ctrl[act_ids_[i]] = tau[i];
-        }
-
-        // === 5. 记录日志：时间、实际关节角、目标角 ===
-        double t = d->time;
-        time_log_.push_back(t);
-
-        std::array<double, kRightArmDOF> q_frame{};
-        std::array<double, kRightArmDOF> q_des_frame{};
-
-        for (int i = 0; i < kRightArmDOF; ++i) {
-            q_frame[i]     = q[i];
-            q_des_frame[i] = ref.q_d[i];
-        }
-        q_log_.push_back(q_frame);
-        q_des_log_.push_back(q_des_frame);
-
-        // === 6. 打印误差（比如每 100 步打印一次） ===
-        if (++print_counter_ % 100 == 0) {
-            Eigen::VectorXd q_des = ref.q_d;
-            Eigen::VectorXd err   = q_des - q;
-
-            std::printf("[Err] ");
-            for (int i = 0; i < kRightArmDOF; ++i) {
-                std::printf("J%d=%.4f ", i, err[i]);
-            }
-            std::printf(" | ||e||=%.4f\n", err.norm());
-        }
-    }
-
-    void onReset(mjModel* m, mjData* d) override {
-        std::printf("[MyViewer] onReset done\n");
-
-        time_log_.clear();
-        q_log_.clear();
-        q_des_log_.clear();
-    }
-};
-
-
-// === 画图函数实现：7 个子图，目标 vs 实际 ===
-void MyViewer::plotJointResponse(const std::string& filename)
 {
-    if (time_log_.empty() || q_log_.empty() || q_des_log_.empty()) {
+    const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml";
-        std::printf("[MyViewer] No log data, skip plotting.\n");
+    auto current = std::filesystem::current_path();
-        return;
+    while (!current.empty()) {
+        if (std::filesystem::exists(current / marker)) {
+            return current;
+        }
+        const auto parent = current.parent_path();
+        if (parent == current) {
+            break;
+        }
+        current = parent;
     }
-
+    return {};
-    const std::vector<double>& t = time_log_;
-
-    plt::figure();
-
-    for (int j = 0; j < kRightArmDOF; ++j) {
-        // 为第 j 个关节准备数据
-        std::vector<double> q_j(t.size());
-        std::vector<double> qd_j(t.size());
-
-        for (size_t k = 0; k < t.size(); ++k) {
-            q_j[k]  = q_log_[k][j];
-            qd_j[k] = q_des_log_[k][j];
-        }
-
-        // 子图：7 行 1 列，第 j+1 个
-        plt::subplot(kRightArmDOF, 1, j + 1);
-
-        plt::named_plot("q_des", t, qd_j);
-        plt::named_plot("q",     t, q_j);
-
-        if (j == 0) {
-            plt::legend();
-        }
-
-        char title[64];
-        std::snprintf(title, sizeof(title), "Joint %d", j);
-        plt::title(title);
-
-        if (j == kRightArmDOF - 1) {
-            plt::xlabel("Time [s]");
-        }
-        plt::ylabel("Angle [rad]");
-        plt::grid(true);
-    }
-
-    // plt::tight_layout();
-    plt::show();
-
-    plt::save(filename);
-    std::printf("[MyViewer] Joint response figure saved to %s\n", filename.c_str());
 }
 
-// === gtest ===
+std::string defaultModelPath()
-TEST(mujoco_viewer_test, test_view_and_pid)
 {
+    const auto root = findProjectRoot();
+    if (root.empty()) {
+        return "model/xiaoyan_description/dual_arm.xml";
+    }
+    return (root / "model/xiaoyan_description/dual_arm.xml").string();
+}
 
-    MyViewer viewer("/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml");
+} // namespace
 
-    std::array<double, 7> q = {
+TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
-        M_PI/6, 1.26, M_PI/12,
+{
-        M_PI/4, 0.22, 0.65, 0.66
+    const char* model_path_env = std::getenv("MUJOCO_VIEWER_TEST_MODEL");
-    };
+    const std::string model_path =
-    viewer.moveJ(q);
+        model_path_env != nullptr && model_path_env[0] != '\0'
+            ? model_path_env
+            : defaultModelPath();
 
+    auto world = std::make_shared<cmvr::simulate::MujocoWorld>();
+    cmvr::simulate::MujocoWorld::Options options;
+    options.model_path = model_path;
+    options.realtime_factor = 1.0;
+
+    ASSERT_TRUE(world->load(options)) << world->lastError();
+    ASSERT_TRUE(world->start()) << world->lastError();
+
+    std::cout << "MujocoWorld loaded: " << model_path << std::endl;
+    std::cout << "Close the MuJoCo window to exit." << std::endl;
+
+    cmvr::MuJocoViewer viewer(world);
     viewer.run();
 
-    // // 仿真结束后画 7 个关节的响应曲线
+    world->stop();
-    viewer.plotJointResponse("right_arm_joint_response.png");
 }
-

cmvr-es/simulate/mujoco/mujoco_viewer/src/platform_ui_adapter.cc

@@ -1,247 +0,0 @@
-// Copyright 2023 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h"
-#include <chrono>
-
-namespace mujoco {
-PlatformUIAdapter::PlatformUIAdapter() {
-  mjr_defaultContext(&con_);
-}
-
-void PlatformUIAdapter::FreeMjrContext() {
-  mjr_freeContext(&con_);
-}
-
-bool PlatformUIAdapter::RefreshMjrContext(const mjModel* m, int fontscale) {
-  if (m != last_model_ || fontscale != last_fontscale_) {
-    mjr_makeContext(m, &con_, fontscale);
-    last_model_ = m;
-    last_fontscale_ = fontscale;
-    return true;
-  }
-  return false;
-}
-
-bool PlatformUIAdapter::EnsureContextSize() {
-  return false;
-}
-
-void PlatformUIAdapter::OnFilesDrop(int count, const char** paths) {
-  state_.type = mjEVENT_FILESDROP;
-  state_.dropcount = count;
-  state_.droppaths = paths;
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-
-  // remove paths pointer from mjuiState since we don't own it
-  state_.dropcount = 0;
-  state_.droppaths = nullptr;
-}
-
-void PlatformUIAdapter::OnKey(int key, int scancode, int act) {
-  // translate API-specific key code
-  int mj_key = TranslateKeyCode(key);
-
-  // release: nothing to do
-  if (!IsKeyDownEvent(act)) {
-    return;
-  }
-
-  // update state
-  UpdateMjuiState();
-
-  // set key info
-  state_.type = mjEVENT_KEY;
-  state_.key = mj_key;
-  state_.keytime = std::chrono::duration<double>(
-      std::chrono::steady_clock::now().time_since_epoch()).count();
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-
-  last_key_ = mj_key;
-}
-
-void PlatformUIAdapter::OnMouseButton(int button, int act)  {
-  // translate API-specific mouse button code
-  mjtButton mj_button = TranslateMouseButton(button);
-
-  // update state
-  UpdateMjuiState();
-
-  // swap left and right if Alt
-  if (state_.alt) {
-    if (mj_button == mjBUTTON_LEFT) {
-      mj_button = mjBUTTON_RIGHT;
-    } else if (mj_button == mjBUTTON_RIGHT) {
-      mj_button = mjBUTTON_LEFT;
-    }
-  }
-
-  // press
-  if (IsMouseButtonDownEvent(act)) {
-    double now = std::chrono::duration<double>(
-        std::chrono::steady_clock::now().time_since_epoch()).count();
-
-    // detect doubleclick: 250 ms
-    if (mj_button == state_.button && now - state_.buttontime < 0.25) {
-      state_.doubleclick = 1;
-    } else {
-      state_.doubleclick = 0;
-    }
-
-    // set info
-    state_.type = mjEVENT_PRESS;
-    state_.button = mj_button;
-    state_.buttontime = now;
-
-    // start dragging
-    if (state_.mouserect) {
-      state_.dragbutton = state_.button;
-      state_.dragrect = state_.mouserect;
-    }
-  }
-
-  // release
-  else {
-    state_.type = mjEVENT_RELEASE;
-  }
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-
-  // stop dragging after application processing
-  if (state_.type == mjEVENT_RELEASE) {
-    state_.dragrect = 0;
-    state_.dragbutton = 0;
-  }
-}
-
-void PlatformUIAdapter::OnMouseMove(double x, double y) {
-  // no buttons down: nothing to do
-  if (!state_.left && !state_.right && !state_.middle) {
-    return;
-  }
-
-  // update state
-  UpdateMjuiState();
-
-  // set move info
-  state_.type = mjEVENT_MOVE;
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-}
-
-void PlatformUIAdapter::OnScroll(double xoffset, double yoffset) {
-  // update state
-  UpdateMjuiState();
-
-  // set scroll info, scale by buffer-to-window ratio
-  const double buffer_window_ratio =
-      static_cast<double>(GetFramebufferSize().first) / GetWindowSize().first;
-  state_.type = mjEVENT_SCROLL;
-  state_.sx = xoffset * buffer_window_ratio;
-  state_.sy = yoffset * buffer_window_ratio;
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-}
-
-void PlatformUIAdapter::OnWindowRefresh() {
-  state_.type = mjEVENT_REDRAW;
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-}
-
-void PlatformUIAdapter::OnWindowResize(int width, int height) {
-  auto [buf_width, buf_height] = GetFramebufferSize();
-  state_.rect[0].width = buf_width;
-  state_.rect[0].height = buf_height;
-  if (state_.nrect < 1) state_.nrect = 1;
-
-  // update window layout
-  if (layout_callback_) {
-    layout_callback_(&state_);
-  }
-
-  // update state
-  UpdateMjuiState();
-
-  // set resize info
-  state_.type = mjEVENT_RESIZE;
-
-  // stop dragging
-  state_.dragbutton = 0;
-  state_.dragrect = 0;
-
-  // application-specific processing
-  if (event_callback_) {
-    event_callback_(&state_);
-  }
-}
-
-void PlatformUIAdapter::UpdateMjuiState() {
-  // mouse buttons
-  state_.left = IsLeftMouseButtonPressed();
-  state_.right = IsRightMouseButtonPressed();
-  state_.middle = IsMiddleMouseButtonPressed();
-
-  // keyboard modifiers
-  state_.control = IsCtrlKeyPressed();
-  state_.shift = IsShiftKeyPressed();
-  state_.alt = IsAltKeyPressed();
-
-  // swap left and right if Alt
-  if (state_.alt) {
-    int tmp = state_.left;
-    state_.left = state_.right;
-    state_.right = tmp;
-  }
-
-  // get mouse position, scale by buffer-to-window ratio
-  auto [x, y] = GetCursorPosition();
-  const double buffer_window_ratio =
-      static_cast<double>(GetFramebufferSize().first) / GetWindowSize().first;
-  x *= buffer_window_ratio;
-  y *= buffer_window_ratio;
-
-  // invert y to match OpenGL convention
-  y = state_.rect[0].height - y;
-
-  // save
-  state_.dx = x - state_.x;
-  state_.dy = y - state_.y;
-  state_.x = x;
-  state_.y = y;
-
-  // find mouse rectangle
-  state_.mouserect = mjr_findRect(mju_round(x), mju_round(y), state_.nrect-1, state_.rect+1) + 1;
-}
-}  // namespace mujoco

cmvr-es/simulate/mujoco/mujoco_world/CMakeLists.txt

@@ -0,0 +1,15 @@
+add_library(mujoco_world SHARED
+        src/mujoco_world.cpp
+)
+
+target_include_directories(mujoco_world PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(mujoco_world PUBLIC
+        cmvr_es::proto
+        cmvr_es::common
+        mujoco
+        pthread
+)
+
+add_library(cmvr_es::mujoco_world ALIAS mujoco_world)
+install(TARGETS mujoco_world LIBRARY DESTINATION lib)

cmvr-es/simulate/mujoco/mujoco_world/include/mujoco_world.h

@@ -0,0 +1,148 @@
+#ifndef CMVR_ES_MUJOCO_WORLD_H
+#define CMVR_ES_MUJOCO_WORLD_H
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h"
+#include "devices/abstract_device.h"
+#include <mujoco/mujoco.h>
+
+namespace cmvr::simulate {
+
+class MujocoWorld {
+public:
+    struct JointSpec {
+        std::string joint_name;
+        bool allow_state_control_fallback{false};
+    };
+
+    struct Options {
+        std::string model_path;
+        std::vector<JointSpec> joints;
+        double timestep_s{0.0};
+        double realtime_factor{1.0};
+        bool require_actuator{false};
+        bool allow_state_control_fallback{false};
+    };
+
+    MujocoWorld() = default;
+    explicit MujocoWorld(Options options);
+    ~MujocoWorld();
+
+    MujocoWorld(const MujocoWorld&) = delete;
+    MujocoWorld& operator=(const MujocoWorld&) = delete;
+
+    bool load(const Options& options);
+    bool start();
+    void stop();
+    void reset();
+
+    bool isLoaded() const;
+    bool isRunning() const;
+    double timestep() const;
+    const std::string& lastError() const;
+    const std::string& modelPath() const;
+
+    std::vector<std::string> jointNames() const;
+    bool hasJoint(const std::string& joint_name) const;
+
+    bool getJointPosition(const std::string& joint_name, double& position) const;
+    bool getJointVelocity(const std::string& joint_name, double& velocity) const;
+    bool setJointPosition(const std::string& joint_name, double position);
+    bool setJointVelocity(const std::string& joint_name, double velocity);
+    bool setJointTargetPosition(const std::string& joint_name, double position);
+    bool setJointTargetState(const std::string& joint_name,
+                             double position,
+                             double velocity);
+    bool setJointTargetPositions(const std::vector<std::string>& joint_names,
+                                 const std::vector<double>& positions);
+    bool setJointTargetStates(const std::vector<std::string>& joint_names,
+                              const std::vector<double>& positions,
+                              const std::vector<double>& velocities);
+    bool setJointTargetVelocity(const std::string& joint_name, double velocity);
+
+    mjModel* model();
+    mjData* data();
+    const mjModel* model() const;
+    const mjData* data() const;
+    std::mutex& mutex() const;
+
+private:
+    enum class TargetMode {
+        HoldPosition,
+        ActuatorPosition,
+        ActuatorVelocity,
+        DirectPosition,
+        DirectVelocity,
+        DirectState
+    };
+
+    struct JointHandle {
+        int joint_id{-1};
+        int qpos_adr{-1};
+        int qvel_adr{-1};
+        int actuator_id{-1};
+        bool allow_state_control_fallback{false};
+        TargetMode mode{TargetMode::HoldPosition};
+        double position_reference{0.0};
+        double target_position{0.0};
+        double target_velocity{0.0};
+    };
+
+    static mjModel* loadModelFile(const std::string& model_path, std::string& error);
+
+    bool buildJointMapLocked(const std::vector<JointSpec>& joint_specs);
+    bool addJointLocked(const JointSpec& joint_spec);
+    int findJointActuatorLocked(const std::string& joint_name, int joint_id) const;
+    bool findJointLocked(const std::string& joint_name, JointHandle*& joint);
+    bool findJointLocked(const std::string& joint_name, const JointHandle*& joint) const;
+    void setLastErrorLocked(const std::string& error) const;
+    void clearModelLocked();
+
+    void simulationLoop();
+    void applyControlLocked();
+    void applyDirectControlLocked(double dt);
+
+    mutable std::mutex mutex_;
+    Options options_;
+    mjModel* model_{nullptr};
+    mjData* data_{nullptr};
+    std::unordered_map<std::string, JointHandle> joints_;
+    mutable std::string last_error_;
+    std::thread simulation_thread_;
+    std::atomic<bool> stop_requested_{false};
+    std::atomic<bool> running_{false};
+};
+
+class MujocoWorldDevice final : public device::AbstractDevice {
+public:
+    explicit MujocoWorldDevice(config::MujocoWorldConfig config);
+    ~MujocoWorldDevice() override;
+
+    device::DeviceKind kind() const noexcept override { return device::DeviceKind::MujocoWorld; }
+    std::string typeName() const override { return "MujocoWorldDevice"; }
+
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+
+    std::shared_ptr<MujocoWorld> world() const { return world_; }
+    static std::shared_ptr<MujocoWorld> worldFor(const std::string& id);
+
+private:
+    config::MujocoWorldConfig config_;
+    std::shared_ptr<MujocoWorld> world_;
+
+    static std::mutex registry_mutex_;
+    static std::unordered_map<std::string, std::weak_ptr<MujocoWorld>> registry_;
+};
+
+} // namespace cmvr::simulate
+
+#endif // CMVR_ES_MUJOCO_WORLD_H

cmvr-es/simulate/mujoco/mujoco_world/src/mujoco_world.cpp

@@ -0,0 +1,776 @@
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+
+#include <algorithm>
+#include <cctype>
+#include <chrono>
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+#include <filesystem>
+#include <limits>
+#include <utility>
+
+#include "common/base/logging/logger.h"
+#include "common/config/config_files.h"
+
+namespace cmvr::simulate {
+namespace {
+
+constexpr double kDefaultTimestep = 0.001;
+
+bool isScalarJoint(const mjModel* model, const int joint_id)
+{
+    if (model == nullptr || joint_id < 0 || joint_id >= model->njnt) {
+        return false;
+    }
+    return model->jnt_type[joint_id] == mjJNT_HINGE ||
+           model->jnt_type[joint_id] == mjJNT_SLIDE;
+}
+
+std::string lowerCopy(std::string value)
+{
+    std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) {
+        return static_cast<char>(std::tolower(c));
+    });
+    return value;
+}
+
+bool containsToken(const std::string& value, const std::string& token)
+{
+    return lowerCopy(value).find(token) != std::string::npos;
+}
+
+bool actuatorTargetsJoint(const mjModel* model, const int actuator_id, const int joint_id)
+{
+    if (model == nullptr || actuator_id < 0 || actuator_id >= model->nu) {
+        return false;
+    }
+    return model->actuator_trntype[actuator_id] == mjTRN_JOINT &&
+           model->actuator_trnid[2 * actuator_id] == joint_id;
+}
+
+int actuatorPriority(const mjModel* model,
+                     const int actuator_id,
+                     const std::string& joint_name)
+{
+    const char* actuator_name = mj_id2name(model, mjOBJ_ACTUATOR, actuator_id);
+    const std::string name = actuator_name == nullptr ? "" : actuator_name;
+    if (name == joint_name) {
+        return 0;
+    }
+    if (containsToken(name, "pos") || containsToken(name, "position")) {
+        return 1;
+    }
+    if (containsToken(name, "motor") || containsToken(name, "act")) {
+        return 2;
+    }
+    return 3;
+}
+
+double clampControl(const mjModel* model, const int actuator_id, const double value)
+{
+    if (model == nullptr || actuator_id < 0 || actuator_id >= model->nu) {
+        return value;
+    }
+    if (!model->actuator_ctrllimited[actuator_id]) {
+        return value;
+    }
+    const double lower = model->actuator_ctrlrange[2 * actuator_id];
+    const double upper = model->actuator_ctrlrange[2 * actuator_id + 1];
+    return std::clamp(value, lower, upper);
+}
+
+} // namespace
+
+std::mutex MujocoWorldDevice::registry_mutex_;
+std::unordered_map<std::string, std::weak_ptr<MujocoWorld>> MujocoWorldDevice::registry_;
+
+MujocoWorld::MujocoWorld(Options options)
+{
+    load(options);
+}
+
+MujocoWorld::~MujocoWorld()
+{
+    stop();
+    std::lock_guard<std::mutex> lock(mutex_);
+    clearModelLocked();
+}
+
+bool MujocoWorld::load(const Options& options)
+{
+    stop();
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    clearModelLocked();
+    options_ = options;
+
+    if (options_.model_path.empty()) {
+        setLastErrorLocked("MujocoWorld model_path is empty");
+        CMVR_LOG(ERROR) << last_error_;
+        return false;
+    }
+
+    std::string error;
+    model_ = loadModelFile(options_.model_path, error);
+    if (model_ == nullptr) {
+        setLastErrorLocked(error);
+        CMVR_LOG(ERROR) << "[MujocoWorld] load model failed: " << last_error_;
+        return false;
+    }
+
+    if (options_.timestep_s > 0.0) {
+        model_->opt.timestep = options_.timestep_s;
+    }
+
+    data_ = mj_makeData(model_);
+    if (data_ == nullptr) {
+        setLastErrorLocked("mj_makeData failed");
+        CMVR_LOG(ERROR) << "[MujocoWorld] " << last_error_;
+        clearModelLocked();
+        return false;
+    }
+
+    if (!buildJointMapLocked(options_.joints)) {
+        clearModelLocked();
+        return false;
+    }
+
+    mj_forward(model_, data_);
+    setLastErrorLocked("");
+    CMVR_LOG(INFO) << "[MujocoWorld] loaded model=" << options_.model_path
+                   << ", joints=" << joints_.size()
+                   << ", timestep=" << model_->opt.timestep;
+    return true;
+}
+
+bool MujocoWorld::start()
+{
+    if (running_.load()) {
+        return true;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (model_ == nullptr || data_ == nullptr) {
+            setLastErrorLocked("MujocoWorld is not loaded");
+            CMVR_LOG(ERROR) << "[MujocoWorld] " << last_error_;
+            return false;
+        }
+    }
+
+    stop_requested_.store(false);
+    running_.store(true);
+    simulation_thread_ = std::thread(&MujocoWorld::simulationLoop, this);
+    return true;
+}
+
+void MujocoWorld::stop()
+{
+    stop_requested_.store(true);
+    if (simulation_thread_.joinable()) {
+        simulation_thread_.join();
+    }
+    running_.store(false);
+}
+
+void MujocoWorld::reset()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (model_ == nullptr || data_ == nullptr) {
+        return;
+    }
+    mj_resetData(model_, data_);
+    mj_forward(model_, data_);
+    for (auto& [_, joint] : joints_) {
+        joint.position_reference = data_->qpos[joint.qpos_adr];
+        joint.target_position = joint.position_reference;
+        joint.target_velocity = 0.0;
+        joint.mode = TargetMode::HoldPosition;
+    }
+}
+
+bool MujocoWorld::isLoaded() const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    return model_ != nullptr && data_ != nullptr;
+}
+
+bool MujocoWorld::isRunning() const
+{
+    return running_.load();
+}
+
+double MujocoWorld::timestep() const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (model_ == nullptr || model_->opt.timestep <= 0.0) {
+        return kDefaultTimestep;
+    }
+    return model_->opt.timestep;
+}
+
+const std::string& MujocoWorld::lastError() const
+{
+    return last_error_;
+}
+
+const std::string& MujocoWorld::modelPath() const
+{
+    return options_.model_path;
+}
+
+std::vector<std::string> MujocoWorld::jointNames() const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::string> names;
+    names.reserve(joints_.size());
+    for (const auto& [name, _] : joints_) {
+        names.push_back(name);
+    }
+    std::sort(names.begin(), names.end());
+    return names;
+}
+
+bool MujocoWorld::hasJoint(const std::string& joint_name) const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    return joints_.find(joint_name) != joints_.end();
+}
+
+bool MujocoWorld::getJointPosition(const std::string& joint_name, double& position) const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    const JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+    position = data_->qpos[joint->qpos_adr];
+    return true;
+}
+
+bool MujocoWorld::getJointVelocity(const std::string& joint_name, double& velocity) const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    const JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+    velocity = data_->qvel[joint->qvel_adr];
+    return true;
+}
+
+bool MujocoWorld::setJointPosition(const std::string& joint_name, const double position)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+    data_->qpos[joint->qpos_adr] = position;
+    data_->qvel[joint->qvel_adr] = 0.0;
+    joint->position_reference = position;
+    joint->target_position = position;
+    joint->target_velocity = 0.0;
+    joint->mode = TargetMode::HoldPosition;
+    mj_forward(model_, data_);
+    return true;
+}
+
+bool MujocoWorld::setJointVelocity(const std::string& joint_name, const double velocity)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+    data_->qvel[joint->qvel_adr] = velocity;
+    joint->position_reference = data_->qpos[joint->qpos_adr];
+    joint->target_velocity = velocity;
+    joint->mode = TargetMode::DirectVelocity;
+    return true;
+}
+
+bool MujocoWorld::setJointTargetPosition(const std::string& joint_name, const double position)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+    if (joint->actuator_id < 0 && !joint->allow_state_control_fallback) {
+        setLastErrorLocked("position target requires an actuator: " + joint_name);
+        return false;
+    }
+    joint->target_position = position;
+    joint->position_reference = position;
+    joint->mode = joint->actuator_id >= 0
+        ? TargetMode::ActuatorPosition
+        : TargetMode::DirectPosition;
+    return true;
+}
+
+bool MujocoWorld::setJointTargetState(const std::string& joint_name,
+                                      const double position,
+                                      const double velocity)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+
+    joint->target_position = position;
+    joint->position_reference = position;
+    joint->target_velocity = velocity;
+    joint->mode = TargetMode::DirectState;
+    return true;
+}
+
+bool MujocoWorld::setJointTargetPositions(const std::vector<std::string>& joint_names,
+                                          const std::vector<double>& positions)
+{
+    if (joint_names.size() != positions.size()) {
+        std::lock_guard<std::mutex> lock(mutex_);
+        setLastErrorLocked("joint target batch size mismatch");
+        return false;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<JointHandle*> joints;
+    joints.reserve(joint_names.size());
+
+    for (const auto& joint_name : joint_names) {
+        JointHandle* joint = nullptr;
+        if (!findJointLocked(joint_name, joint)) {
+            return false;
+        }
+        if (joint->actuator_id < 0 && !joint->allow_state_control_fallback) {
+            setLastErrorLocked("position target requires an actuator: " + joint_name);
+            return false;
+        }
+        joints.push_back(joint);
+    }
+
+    for (std::size_t i = 0; i < joints.size(); ++i) {
+        JointHandle* joint = joints[i];
+        joint->target_position = positions[i];
+        joint->position_reference = positions[i];
+        joint->mode = joint->actuator_id >= 0
+            ? TargetMode::ActuatorPosition
+            : TargetMode::DirectPosition;
+    }
+    return true;
+}
+
+bool MujocoWorld::setJointTargetStates(const std::vector<std::string>& joint_names,
+                                       const std::vector<double>& positions,
+                                       const std::vector<double>& velocities)
+{
+    if (joint_names.size() != positions.size() || joint_names.size() != velocities.size()) {
+        std::lock_guard<std::mutex> lock(mutex_);
+        setLastErrorLocked("joint target state batch size mismatch");
+        return false;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<JointHandle*> joints;
+    joints.reserve(joint_names.size());
+
+    for (const auto& joint_name : joint_names) {
+        JointHandle* joint = nullptr;
+        if (!findJointLocked(joint_name, joint)) {
+            return false;
+        }
+        joints.push_back(joint);
+    }
+
+    for (std::size_t i = 0; i < joints.size(); ++i) {
+        JointHandle* joint = joints[i];
+        joint->target_position = positions[i];
+        joint->position_reference = positions[i];
+        joint->target_velocity = velocities[i];
+        joint->mode = TargetMode::DirectState;
+    }
+    return true;
+}
+
+bool MujocoWorld::setJointTargetVelocity(const std::string& joint_name, const double velocity)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    JointHandle* joint = nullptr;
+    if (!findJointLocked(joint_name, joint)) {
+        return false;
+    }
+    if (joint->mode != TargetMode::DirectVelocity) {
+        joint->position_reference = data_->qpos[joint->qpos_adr];
+    }
+    joint->target_velocity = velocity;
+    joint->mode = TargetMode::DirectVelocity;
+    return true;
+}
+
+mjModel* MujocoWorld::model()
+{
+    return model_;
+}
+
+mjData* MujocoWorld::data()
+{
+    return data_;
+}
+
+const mjModel* MujocoWorld::model() const
+{
+    return model_;
+}
+
+const mjData* MujocoWorld::data() const
+{
+    return data_;
+}
+
+std::mutex& MujocoWorld::mutex() const
+{
+    return mutex_;
+}
+
+mjModel* MujocoWorld::loadModelFile(const std::string& model_path, std::string& error)
+{
+    error.clear();
+    if (model_path.empty()) {
+        error = "empty model filename";
+        return nullptr;
+    }
+
+    const std::filesystem::path path(model_path);
+    if (!std::filesystem::exists(path)) {
+        error = "model file does not exist: " + model_path;
+        return nullptr;
+    }
+
+    if (path.extension() == ".mjb") {
+        mjModel* model = mj_loadModel(model_path.c_str(), nullptr);
+        if (model == nullptr) {
+            error = "could not load binary model: " + model_path;
+        }
+        return model;
+    }
+
+    char load_error[1024] = {};
+    mjModel* model = mj_loadXML(model_path.c_str(), nullptr, load_error, sizeof(load_error));
+    if (model == nullptr) {
+        error = load_error[0] == '\0' ? "could not load XML model: " + model_path : load_error;
+        if (!error.empty() && error.back() == '\n') {
+            error.pop_back();
+        }
+    }
+    return model;
+}
+
+bool MujocoWorld::buildJointMapLocked(const std::vector<JointSpec>& joint_specs)
+{
+    joints_.clear();
+    bool ok = true;
+
+    if (joint_specs.empty()) {
+        for (int joint_id = 0; joint_id < model_->njnt; ++joint_id) {
+            if (!isScalarJoint(model_, joint_id)) {
+                continue;
+            }
+            const char* name = mj_id2name(model_, mjOBJ_JOINT, joint_id);
+            if (name == nullptr || name[0] == '\0') {
+                continue;
+            }
+            JointSpec joint_spec;
+            joint_spec.joint_name = name;
+            joint_spec.allow_state_control_fallback = options_.allow_state_control_fallback;
+            ok = addJointLocked(joint_spec) && ok;
+        }
+    } else {
+        for (const auto& joint_spec : joint_specs) {
+            ok = addJointLocked(joint_spec) && ok;
+        }
+    }
+
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[MujocoWorld] build joint map failed: " << last_error_;
+    }
+    return ok;
+}
+
+bool MujocoWorld::addJointLocked(const JointSpec& joint_spec)
+{
+    const std::string& joint_name = joint_spec.joint_name;
+    if (joint_name.empty()) {
+        setLastErrorLocked("empty Mujoco joint name");
+        return false;
+    }
+
+    const int joint_id = mj_name2id(model_, mjOBJ_JOINT, joint_name.c_str());
+    if (joint_id < 0) {
+        setLastErrorLocked("joint not found in Mujoco model: " + joint_name);
+        return false;
+    }
+    if (!isScalarJoint(model_, joint_id)) {
+        setLastErrorLocked("only hinge/slide scalar joints are supported: " + joint_name);
+        return false;
+    }
+
+    JointHandle joint;
+    joint.joint_id = joint_id;
+    joint.qpos_adr = model_->jnt_qposadr[joint_id];
+    joint.qvel_adr = model_->jnt_dofadr[joint_id];
+    joint.actuator_id = findJointActuatorLocked(joint_name, joint_id);
+    joint.allow_state_control_fallback =
+        joint_spec.allow_state_control_fallback || options_.allow_state_control_fallback;
+    joint.position_reference = data_->qpos[joint.qpos_adr];
+    joint.target_position = joint.position_reference;
+
+    if (options_.require_actuator && joint.actuator_id < 0) {
+        setLastErrorLocked("actuator not found for joint: " + joint_name);
+        return false;
+    }
+
+    joints_[joint_name] = joint;
+    return true;
+}
+
+int MujocoWorld::findJointActuatorLocked(const std::string& joint_name, const int joint_id) const
+{
+    int best_actuator_id = -1;
+    int best_priority = std::numeric_limits<int>::max();
+    int match_count = 0;
+    for (int actuator_id = 0; actuator_id < model_->nu; ++actuator_id) {
+        if (!actuatorTargetsJoint(model_, actuator_id, joint_id)) {
+            continue;
+        }
+        ++match_count;
+        const int priority = actuatorPriority(model_, actuator_id, joint_name);
+        if (priority < best_priority) {
+            best_priority = priority;
+            best_actuator_id = actuator_id;
+        }
+    }
+
+    if (match_count > 1 && best_actuator_id >= 0) {
+        const char* actuator_name = mj_id2name(model_, mjOBJ_ACTUATOR, best_actuator_id);
+        CMVR_LOG(DEBUG) << "[MujocoWorld] multiple actuators target joint=" << joint_name
+                        << ", selected=" << (actuator_name == nullptr ? "" : actuator_name);
+    }
+    return best_actuator_id;
+}
+
+bool MujocoWorld::findJointLocked(const std::string& joint_name, JointHandle*& joint)
+{
+    auto it = joints_.find(joint_name);
+    if (it == joints_.end() || model_ == nullptr || data_ == nullptr) {
+        setLastErrorLocked("joint not available: " + joint_name);
+        joint = nullptr;
+        return false;
+    }
+    joint = &it->second;
+    return true;
+}
+
+bool MujocoWorld::findJointLocked(const std::string& joint_name, const JointHandle*& joint) const
+{
+    auto it = joints_.find(joint_name);
+    if (it == joints_.end() || model_ == nullptr || data_ == nullptr) {
+        setLastErrorLocked("joint not available: " + joint_name);
+        joint = nullptr;
+        return false;
+    }
+    joint = &it->second;
+    return true;
+}
+
+void MujocoWorld::setLastErrorLocked(const std::string& error) const
+{
+    last_error_ = error;
+}
+
+void MujocoWorld::clearModelLocked()
+{
+    joints_.clear();
+    if (data_ != nullptr) {
+        mj_deleteData(data_);
+        data_ = nullptr;
+    }
+    if (model_ != nullptr) {
+        mj_deleteModel(model_);
+        model_ = nullptr;
+    }
+}
+
+void MujocoWorld::simulationLoop()
+{
+    CMVR_LOG(INFO) << "[MujocoWorld] simulation started";
+
+    double dt = kDefaultTimestep;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (model_ != nullptr && model_->opt.timestep > 0.0) {
+            dt = model_->opt.timestep;
+        }
+    }
+
+    const double realtime_factor = options_.realtime_factor > 0.0 ? options_.realtime_factor : 1.0;
+    const auto period = std::chrono::duration<double>(dt / realtime_factor);
+    auto next_tick = std::chrono::steady_clock::now() + period;
+
+    while (!stop_requested_.load()) {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (model_ == nullptr || data_ == nullptr) {
+                break;
+            }
+            applyControlLocked();
+            mj_step(model_, data_);
+            applyDirectControlLocked(dt);
+        }
+
+        std::this_thread::sleep_until(next_tick);
+        next_tick += period;
+        const auto now = std::chrono::steady_clock::now();
+        if (next_tick < now) {
+            next_tick = now + period;
+        }
+    }
+
+    running_.store(false);
+    CMVR_LOG(INFO) << "[MujocoWorld] simulation stopped";
+}
+
+void MujocoWorld::applyControlLocked()
+{
+    for (auto& [_, joint] : joints_) {
+        if (joint.qpos_adr < 0 || joint.qvel_adr < 0) {
+            continue;
+        }
+
+        if (joint.mode == TargetMode::ActuatorVelocity) {
+            joint.position_reference += joint.target_velocity * model_->opt.timestep;
+            data_->ctrl[joint.actuator_id] =
+                clampControl(model_, joint.actuator_id, joint.position_reference);
+        } else if (joint.mode == TargetMode::ActuatorPosition) {
+            joint.position_reference = joint.target_position;
+            data_->ctrl[joint.actuator_id] =
+                clampControl(model_, joint.actuator_id, joint.position_reference);
+        } else if (joint.actuator_id >= 0) {
+            joint.position_reference = data_->qpos[joint.qpos_adr];
+            data_->ctrl[joint.actuator_id] =
+                clampControl(model_, joint.actuator_id, joint.position_reference);
+        }
+    }
+}
+
+void MujocoWorld::applyDirectControlLocked(const double dt)
+{
+    bool updated = false;
+    const double step = dt > 0.0 ? dt : model_->opt.timestep;
+
+    for (auto& [_, joint] : joints_) {
+        if (joint.qpos_adr < 0 || joint.qvel_adr < 0) {
+            continue;
+        }
+
+        if (joint.mode == TargetMode::DirectVelocity) {
+            joint.position_reference += joint.target_velocity * step;
+            data_->qpos[joint.qpos_adr] = joint.position_reference;
+            data_->qvel[joint.qvel_adr] = joint.target_velocity;
+            updated = true;
+        } else if (joint.mode == TargetMode::DirectPosition) {
+            joint.position_reference = joint.target_position;
+            data_->qpos[joint.qpos_adr] = joint.target_position;
+            data_->qvel[joint.qvel_adr] = 0.0;
+            updated = true;
+        } else if (joint.mode == TargetMode::DirectState) {
+            joint.position_reference = joint.target_position;
+            data_->qpos[joint.qpos_adr] = joint.target_position;
+            data_->qvel[joint.qvel_adr] = joint.target_velocity;
+            updated = true;
+        }
+    }
+
+    if (updated) {
+        mj_forward(model_, data_);
+    }
+}
+
+MujocoWorldDevice::MujocoWorldDevice(config::MujocoWorldConfig config)
+    : config_(std::move(config))
+{
+    id_ = config_.id();
+}
+
+MujocoWorldDevice::~MujocoWorldDevice()
+{
+    stop();
+}
+
+bool MujocoWorldDevice::init()
+{
+    if (id_.empty()) {
+        CMVR_LOG(ERROR) << "[MujocoWorldDevice] id is empty";
+        return false;
+    }
+    if (world_ && world_->isLoaded()) {
+        return true;
+    }
+
+    MujocoWorld::Options options;
+    options.model_path = cmvr::ConfigHelper::resolveResourceFile(config_.model_path());
+    options.timestep_s = config_.timestep_s();
+    options.realtime_factor = config_.realtime_factor() > 0.0 ? config_.realtime_factor() : 1.0;
+    options.require_actuator = config_.require_actuator();
+    options.allow_state_control_fallback = config_.allow_state_control_fallback();
+
+    world_ = std::make_shared<MujocoWorld>();
+    if (!world_->load(options)) {
+        CMVR_LOG(ERROR) << "[MujocoWorldDevice] load failed, id=" << id_
+                        << ", error=" << world_->lastError();
+        world_.reset();
+        return false;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(registry_mutex_);
+        registry_[id_] = world_;
+    }
+    CMVR_LOG(INFO) << "[MujocoWorldDevice] initialized: " << id_;
+    return true;
+}
+
+bool MujocoWorldDevice::start()
+{
+    if (!world_ && !init()) {
+        return false;
+    }
+    if (!world_->start()) {
+        CMVR_LOG(ERROR) << "[MujocoWorldDevice] start failed, id=" << id_
+                        << ", error=" << world_->lastError();
+        return false;
+    }
+    return true;
+}
+
+bool MujocoWorldDevice::stop()
+{
+    if (world_) {
+        world_->stop();
+    }
+    return true;
+}
+
+std::shared_ptr<MujocoWorld> MujocoWorldDevice::worldFor(const std::string& id)
+{
+    std::lock_guard<std::mutex> lock(registry_mutex_);
+    const auto it = registry_.find(id);
+    if (it == registry_.end()) {
+        return nullptr;
+    }
+    return it->second.lock();
+}
+
+} // namespace cmvr::simulate

cmvr-es/test/CMakeLists.txt

@@ -0,0 +1,17 @@
+add_executable(mujoco_manual_ui_test
+        mujoco_manual_ui_test.cpp
+)
+
+target_compile_definitions(mujoco_manual_ui_test PRIVATE
+        CMVR_MANUAL_UI_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/cmvr_es.pb.txt"
+)
+
+target_link_libraries(mujoco_manual_ui_test
+        PRIVATE
+        gtest
+        gtest_main
+        pthread
+        cmvr_es::runtime
+        cmvr_es::device::arm
+        cmvr_es::device::camera
+)

cmvr-es/test/mujoco_manual_ui_test.cpp

@@ -0,0 +1,237 @@
+#include <atomic>
+#include <chrono>
+#include <cstdlib>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "common/base/logging/logger.h"
+#include "devices/arm/robot_arm.h"
+#include "devices/camera/abstract_camera.h"
+#include "runtime/include/cmvr_runtime.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+
+namespace {
+
+std::string manualConfigPath()
+{
+    if (const char* env = std::getenv("CMVR_MANUAL_UI_CONFIG_PATH")) {
+        return env;
+    }
+    return CMVR_MANUAL_UI_CONFIG_PATH;
+}
+
+std::string manualArmId()
+{
+    if (const char* env = std::getenv("CMVR_MANUAL_ARM_ID")) {
+        return env;
+    }
+    return "mujoco_right_arm";
+}
+
+std::string manualRealArmId()
+{
+    if (const char* env = std::getenv("CMVR_MANUAL_REAL_ARM_ID")) {
+        return env;
+    }
+    return "right_arm";
+}
+
+bool shouldRunRealArmTest()
+{
+    const char* env = std::getenv("CMVR_RUN_REAL_ARM_TEST");
+    return env != nullptr && std::string(env) == "1";
+}
+
+struct ManualUiRunResult {
+    bool ok = true;
+    std::string error;
+};
+
+ManualUiRunResult runManualUiTest(
+    const std::shared_ptr<cmvr::device::RobotArm>& arm,
+    const std::string& arm_id,
+    const std::atomic_bool& stop_requested,
+    const std::shared_ptr<cmvr::simulate::MujocoWorld>& mujoco_world = nullptr)
+{
+    if (stop_requested.load()) {
+        return {};
+    }
+
+    std::vector<double> q_target = {0, 1, 1.6, 1.6, -2.5, 0.12, 0.12};
+
+    cmvr::device::MotionOptions joint_options;
+    joint_options.velocity = 5.0;
+    joint_options.acceleration = 15.0;
+    joint_options.joint_velocity_limits.assign(7, 2.5);
+
+    CMVR_LOG(INFO) << "[MujocoManualUiTest] moveJ " << arm_id;
+    auto result = arm->moveJ(cmvr::device::JointPositionCommand{q_target}, joint_options);
+    if (!result.ok()) {
+        return {false, result.message};
+    }
+
+    if (stop_requested.load()) {
+        return {};
+    }
+
+    // std::this_thread::sleep_for(std::chrono::seconds(1));
+    //
+    // auto move_l_target = arm->getTcpPose(cmvr::device::FrameType::Base);
+    // // move_l_target.z += 1.50;
+    // move_l_target.ry += 0.5;
+    //
+    // cmvr::device::MotionOptions cartesian_options;
+    // cartesian_options.velocity = 0.19;
+    // cartesian_options.acceleration = 100.0;
+    // cartesian_options.jerk = 500.0;
+    //
+    // CMVR_LOG(INFO) << "[MujocoManualUiTest] moveL +X";
+    // result = arm->moveL(move_l_target, cartesian_options, cmvr::device::FrameType::Base);
+    // if (!result.ok()) {
+    //     return {false, result.message};
+    // }
+    //
+    // if (stop_requested.load()) {
+    //     return {};
+    // }
+
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+    cmvr::device::CartesianVelocity speed_l_velocity;
+    speed_l_velocity.vz = 0.19;
+    // speed_l_velocity.wy = 0.19;
+
+    CMVR_LOG(INFO) << "[MujocoManualUiTest] speedL +X";
+    result = arm->speedL(speed_l_velocity, 100.0, 1.0, cmvr::device::FrameType::Base);
+    if (!result.ok()) {
+        return {false, result.message};
+    }
+
+    // auto last_pose = arm->getTcpPose(cmvr::device::FrameType::Base);
+    // auto last_time = std::chrono::steady_clock::now();
+    // auto read_sim_time = [&]() {
+    //     if (!mujoco_world) {
+    //         return std::numeric_limits<double>::quiet_NaN();
+    //     }
+    //     std::lock_guard<std::mutex> lock(mujoco_world->mutex());
+    //     const auto* data = mujoco_world->data();
+    //     return data ? data->time : std::numeric_limits<double>::quiet_NaN();
+    // };
+    // double last_sim_time = read_sim_time();
+    // const auto speed_l_end_time = last_time + std::chrono::seconds(2);
+    // while (!stop_requested.load() && std::chrono::steady_clock::now() < speed_l_end_time) {
+    //     std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    //
+    //     const auto now = std::chrono::steady_clock::now();
+    //     const auto pose = arm->getTcpPose(cmvr::device::FrameType::Base);
+    //     const auto command_twist = arm->getSpeedLCommandTwistBase();
+    //     const double command_norm =
+    //         std::sqrt(command_twist.vx * command_twist.vx +
+    //                   command_twist.vy * command_twist.vy +
+    //                   command_twist.vz * command_twist.vz);
+    //     const double dt = std::chrono::duration<double>(now - last_time).count();
+    //     const double sim_time = read_sim_time();
+    //     const double sim_dt =
+    //         std::isfinite(sim_time) && std::isfinite(last_sim_time)
+    //             ? sim_time - last_sim_time
+    //             : std::numeric_limits<double>::quiet_NaN();
+    //     const double sim_realtime_ratio =
+    //         dt > 1e-6 && std::isfinite(sim_dt)
+    //             ? sim_dt / dt
+    //             : std::numeric_limits<double>::quiet_NaN();
+    //     if (dt > 1e-6) {
+    //         const double vx = (pose.x - last_pose.x) / dt;
+    //         const double vy = (pose.y - last_pose.y) / dt;
+    //         const double vz = (pose.z - last_pose.z) / dt;
+    //         const double v_norm = std::sqrt(vx * vx + vy * vy + vz * vz);
+    //         CMVR_LOG(INFO) << "[MujocoManualUiTest] tcp velocity base: vx="
+    //                        << vx << ", vy=" << vy << ", vz=" << vz
+    //                        << ", norm=" << v_norm
+    //                        << ", command_vx=" << command_twist.vx
+    //                        << ", command_vy=" << command_twist.vy
+    //                        << ", command_vz=" << command_twist.vz
+    //                        << ", command_norm=" << command_norm
+    //                        << ", wall_dt=" << dt;
+    //         if (mujoco_world) {
+    //             CMVR_LOG(INFO) << "[MujocoManualUiTest] sim timing: sim_dt="
+    //                            << sim_dt
+    //                            << ", sim_realtime_ratio=" << sim_realtime_ratio;
+    //         }
+    //     }
+    //
+    //     last_pose = pose;
+    //     last_time = now;
+    //     last_sim_time = sim_time;
+    // }
+    // (void)arm->stopL(10);
+
+    return {};
+}
+
+} // namespace
+
+TEST(MujocoManualUiTest, RunMujocoArm)
+{
+    cmvr::Runtime runtime;
+    ASSERT_TRUE(runtime.init(manualConfigPath()));
+    ASSERT_TRUE(runtime.startTasks());
+
+    auto& devices = runtime.deviceManager();
+
+    const std::string arm_id = manualArmId();
+    auto arm = devices.getDevice<cmvr::device::RobotArm>(arm_id);
+    auto mujoco_camera = devices.getDevice<cmvr::device::AbstractCamera>("mujoco_hand_cam");
+    auto viewer = runtime.mujocoViewer("mujoco_viewer");
+    auto mujoco_world = cmvr::simulate::MujocoWorldDevice::worldFor("mujoco_world");
+
+    ASSERT_TRUE(arm);
+    ASSERT_TRUE(viewer);
+
+    std::atomic_bool stop_requested{false};
+    ManualUiRunResult worker_result;
+
+    std::thread worker([&]() {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+        worker_result = runManualUiTest(arm, arm_id, stop_requested, mujoco_world);
+        if (mujoco_camera && !stop_requested.load()) {
+            mujoco_camera->start();
+        }
+    });
+
+    viewer->runOnMainThread();
+    stop_requested.store(true);
+    if (worker.joinable()) {
+        worker.join();
+    }
+
+    EXPECT_TRUE(worker_result.ok) << worker_result.error;
+    runtime.shutdown();
+}
+
+TEST(MujocoManualUiTest, RunRealArm)
+{
+    if (!shouldRunRealArmTest()) {
+        GTEST_SKIP() << "Set CMVR_RUN_REAL_ARM_TEST=1 to run the real arm manual UI test.";
+    }
+
+    cmvr::Runtime runtime;
+    ASSERT_TRUE(runtime.init(manualConfigPath()));
+    ASSERT_TRUE(runtime.startTasks());
+
+    const std::string arm_id = manualRealArmId();
+    auto arm = runtime.deviceManager().getDevice<cmvr::device::RobotArm>(arm_id);
+    ASSERT_TRUE(arm);
+
+    std::atomic_bool stop_requested{false};
+    const ManualUiRunResult result = runManualUiTest(arm, arm_id, stop_requested);
+
+    EXPECT_TRUE(result.ok) << result.error;
+    runtime.shutdown();
+}

protos/cmvr/config/mujoco_config/mujoco_world_config.proto

@@ -0,0 +1,27 @@
+syntax = "proto3";
+package cmvr.config;
+
+message MujocoWorldConfig {
+  string id = 1;
+  string model_path = 2;
+  double timestep_s = 3;
+  double realtime_factor = 4;
+  bool require_actuator = 5;
+  bool allow_state_control_fallback = 6;
+}
+
+message MujocoWorldRootConfig {
+  repeated MujocoWorldConfig worlds = 1;
+}
+
+message MujocoViewerConfig {
+  string id = 1;
+  string world_id = 2;
+  double camera_distance = 3;
+  double camera_azimuth = 4;
+  double camera_elevation = 5;
+}
+
+message MujocoViewerRootConfig {
+  repeated MujocoViewerConfig viewers = 1;
+}