diff --git a/doc/fun.dio b/doc/fun.dio index c7bd992..8d68acb 100644 --- a/doc/fun.dio +++ b/doc/fun.dio @@ -1,6 +1,6 @@ - + @@ -22,10 +22,10 @@ - + - + @@ -53,7 +53,7 @@ - + diff --git a/doc/func.png b/doc/func.png index 11ac6d2..c2a2505 100644 Binary files a/doc/func.png and b/doc/func.png differ diff --git a/readme.md b/readme.md index f31bab6..0010e76 100644 --- a/readme.md +++ b/readme.md @@ -11,6 +11,7 @@ - `example`: Example programs ## Supported Unitree sdk2 Messages: +**Current version only supports low-level development, mainly used for sim to real verification of controller** - `LowCmd`: Motor control commands - `LowState`: Motor state information - `SportModeState`: Robot position and velocity data @@ -35,6 +36,7 @@ Note: ## C++ Simulator (simulate) ### 1. Dependencies #### unitree_sdk2 +It is recommended to install `unitree_sdk2` in `/opt/unitree_robotics` path. ```bash git clone https://github.com/unitreerobotics/unitree_sdk2.git cd unitree_sdk2/ @@ -44,7 +46,8 @@ cmake .. -DCMAKE_INSTALL_PREFIX=/opt/unitree_robotics sudo make install ``` For more details, see: https://github.com/unitreerobotics/unitree_sdk2 -#### mujoco >= 3.0.0 +#### mujoco +Current version is tested in mujoco-3.2.7 ```bash sudo apt install libglfw3-dev libxinerama-dev libxcursor-dev libxi-dev ``` diff --git a/readme_zh.md b/readme_zh.md index e8ea566..c7b5729 100644 --- a/readme_zh.md +++ b/readme_zh.md @@ -13,6 +13,7 @@ - `example`: 例程 ## 支持的 Unitree sdk2 消息： +**当前版本仅支持底层开发，主要用于控制器的 sim to real 验证** - `LowCmd`: 电机控制指令 - `LowState`：电机状态 - `SportModeState`：机器人位置和速度 @@ -36,6 +37,7 @@ ## c++ 仿真器 (simulate) ### 1. 依赖 #### unitree_sdk2 +推荐将 `unitree_sdk2` 安装在 `/opt/unitree_robotics` 路径下。 ```bash git clone https://github.com/unitreerobotics/unitree_sdk2.git cd unitree_sdk2/ @@ -45,7 +47,8 @@ cmake .. -DCMAKE_INSTALL_PREFIX=/opt/unitree_robotics sudo make install ``` 详细见：https://github.com/unitreerobotics/unitree_sdk2 -#### mujoco >= 3.0.0 +#### mujoco +当前版本基于 mujoco-3.2.7 测试 ```bash sudo apt install libglfw3-dev libxinerama-dev libxcursor-dev libxi-dev ``` diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 1436e19..4f124e0 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -11,12 +11,12 @@ find_package(mujoco REQUIRED) find_package(unitree_sdk2 REQUIRED) -FILE (GLOB SIM_SRC - src/joystick/joystick.cc - src/mujoco/*.cc - src/unitree_sdk2_bridge/*.cc) +file(GLOB SIM_SRC + src/joystick/joystick.cc + src/mujoco/*.cc + src/unitree_sdk2_bridge/*.cc) -set(SIM_DEPENDENCIES +set(SIM_DEPENDENCIES pthread mujoco::mujoco glfw @@ -25,11 +25,11 @@ set(SIM_DEPENDENCIES add_executable(unitree_mujoco ${SIM_SRC} src/main.cc) -target_link_libraries(unitree_mujoco ${SIM_DEPENDENCIES}) +target_link_libraries(unitree_mujoco ${SIM_DEPENDENCIES}) add_executable(test test/test_unitree_sdk2.cpp) target_link_libraries(test unitree_sdk2) add_executable(jstest src/joystick/jstest.cc src/joystick/joystick.cc) -SET(CMAKE_BUILD_TYPE Release) \ No newline at end of file +set(CMAKE_BUILD_TYPE Release) diff --git a/simulate/config.yaml b/simulate/config.yaml index b50a705..d2131bc 100644 --- a/simulate/config.yaml +++ b/simulate/config.yaml @@ -4,7 +4,7 @@ robot_scene: "scene.xml" # Robot scene, /unitree_robots/[robot]/scene.xml domain_id: 1 # Domain id interface: "lo" # Interface -use_joystick: 1 # Simulate Unitree WirelessController using a gamepad +use_joystick: 0 # Simulate Unitree WirelessController using a gamepad joystick_type: "xbox" # support "xbox" and "switch" gamepad layout joystick_device: "/dev/input/js0" # Device path joystick_bits: 16 # Some game controllers may only have 8-bit accuracy diff --git a/simulate/src/mujoco/array_safety.h b/simulate/src/mujoco/array_safety.h index e8dd0d7..4207a9d 100644 --- a/simulate/src/mujoco/array_safety.h +++ b/simulate/src/mujoco/array_safety.h @@ -30,76 +30,89 @@ // // 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 -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 -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 -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 -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 -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 -static inline char* strcpy_arr(char (&dest)[N], const char* src) { +namespace mujoco +{ + namespace sample_util { - std::size_t i = 0; - for (; src[i] && i < N - 1; ++i) { - dest[i] = src[i]; + + // returns sizeof(arr) + // use instead of sizeof() to avoid unintended array-to-pointer decay + template + static constexpr std::size_t sizeof_arr(const T (&arr)[N]) + { + return sizeof(arr); } - dest[i] = '\0'; - } - return &dest[0]; -} -} // namespace sample_util -} // namespace mujoco + // like std::strcmp but it will not read beyond the bound of either lhs or rhs + template + static inline int strcmp_arr(const char (&lhs)[N1], const char (&rhs)[N2]) + { + return std::strncmp(lhs, rhs, std::min(N1, N2)); + } -#endif // MUJOCO_SAMPLE_ARRAY_SAFETY_H_ + // like std::strlen but it will not read beyond the bound of str + // if str is not null-terminated, returns sizeof(str) + template + 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 + 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 + 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 + 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_ diff --git a/simulate/src/mujoco/glfw_adapter.cc b/simulate/src/mujoco/glfw_adapter.cc index 53da6c0..61080d6 100644 --- a/simulate/src/mujoco/glfw_adapter.cc +++ b/simulate/src/mujoco/glfw_adapter.cc @@ -26,231 +26,262 @@ #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); +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; } - return success; - }(); - return is_initialized; -} -GlfwAdapter& GlfwAdapterFromWindow(GLFWwindow* window) { - return *static_cast(Glfw().glfwGetWindowUserPointer(window)); -} -} // namespace + GlfwAdapter &GlfwAdapterFromWindow(GLFWwindow *window) + { + return *static_cast(Glfw().glfwGetWindowUserPointer(window)); + } + } // namespace -GlfwAdapter::GlfwAdapter() { - if (MaybeGlfwInit() != GLFW_TRUE) { - mju_error("could not initialize GLFW"); - } + GlfwAdapter::GlfwAdapter() + { + if (MaybeGlfwInit() != GLFW_TRUE) + { + mju_error("could not initialize GLFW"); + } - // multisampling - Glfw().glfwWindowHint(GLFW_SAMPLES, 4); - Glfw().glfwWindowHint(GLFW_VISIBLE, 1); + // multisampling + Glfw().glfwWindowHint(GLFW_SAMPLES, 4); + Glfw().glfwWindowHint(GLFW_VISIBLE, 1); - // get video mode and save - vidmode_ = *Glfw().glfwGetVideoMode(Glfw().glfwGetPrimaryMonitor()); + // 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"); - } + // 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); + // 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) { + // 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); - }); + 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 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 GlfwAdapter::GetFramebufferSize() const { - int width, height; - Glfw().glfwGetFramebufferSize(window_, &width, &height); - return {width, height}; -} - -std::pair 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(); + // make context current + Glfw().glfwMakeContextCurrent(window_); } + + GlfwAdapter::~GlfwAdapter() + { + FreeMjrContext(); + Glfw().glfwMakeContextCurrent(nullptr); + Glfw().glfwDestroyWindow(window_); + } + + std::pair 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 GlfwAdapter::GetFramebufferSize() const + { + int width, height; + Glfw().glfwGetFramebufferSize(window_, &width, &height); + return {width, height}; + } + + std::pair 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); + Glfw().glfwSwapInterval(enabled); #endif -} + } -void GlfwAdapter::SetWindowTitle(const char* title) { - Glfw().glfwSetWindowTitle(window_, title); -} + void GlfwAdapter::SetWindowTitle(const char *title) + { + Glfw().glfwSetWindowTitle(window_, title); + } -bool GlfwAdapter::ShouldCloseWindow() const { - return Glfw().glfwWindowShouldClose(window_); -} + bool GlfwAdapter::ShouldCloseWindow() const + { + return Glfw().glfwWindowShouldClose(window_); + } -void GlfwAdapter::SwapBuffers() { + void GlfwAdapter::SwapBuffers() + { #ifdef __APPLE__ - if (core_video_.has_value()) { - core_video_->WaitForDisplayRefresh(); - } + 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); + Glfw().glfwSwapBuffers(window_); } - // 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); + 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); + } - // switch - Glfw().glfwSetWindowMonitor(window_, Glfw().glfwGetPrimaryMonitor(), 0, - 0, vidmode_.width, vidmode_.height, - vidmode_.refreshRate); + // 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::IsKeyPressed(int key) const { - return Glfw().glfwGetKey(window_, key) == 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; + bool GlfwAdapter::IsLeftMouseButtonPressed() const + { + return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS; } - return mjBUTTON_NONE; -} -} // namespace mujoco + + 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 diff --git a/simulate/src/mujoco/glfw_adapter.h b/simulate/src/mujoco/glfw_adapter.h index f47e897..6e17a76 100644 --- a/simulate/src/mujoco/glfw_adapter.h +++ b/simulate/src/mujoco/glfw_adapter.h @@ -26,55 +26,55 @@ #include "glfw_corevideo.h" #endif -namespace mujoco { -class GlfwAdapter : public PlatformUIAdapter { - public: - GlfwAdapter(); - ~GlfwAdapter() override; +namespace mujoco +{ + class GlfwAdapter : public PlatformUIAdapter + { + public: + GlfwAdapter(); + ~GlfwAdapter() override; - std::pair GetCursorPosition() const override; - double GetDisplayPixelsPerInch() const override; - std::pair GetFramebufferSize() const override; - std::pair 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; + std::pair GetCursorPosition() const override; + double GetDisplayPixelsPerInch() const override; + std::pair GetFramebufferSize() const override; + std::pair 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 IsLeftMouseButtonPressed() const override; + bool IsMiddleMouseButtonPressed() const override; + bool IsRightMouseButtonPressed() const override; - bool IsAltKeyPressed() const override; - bool IsCtrlKeyPressed() const override; - bool IsShiftKeyPressed() const override; + bool IsAltKeyPressed() const override; + bool IsCtrlKeyPressed() const override; + bool IsShiftKeyPressed() const override; - bool IsKeyPressed(int key) const override; + bool IsMouseButtonDownEvent(int act) const override; + bool IsKeyDownEvent(int act) 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; - int TranslateKeyCode(int key) const override; - mjtButton TranslateMouseButton(int button) const override; + private: + GLFWvidmode vidmode_; + GLFWwindow *window_; - private: - GLFWvidmode vidmode_; - GLFWwindow* window_; - - // store last window information when going to full screen - std::pair window_pos_; - std::pair window_size_; + // store last window information when going to full screen + std::pair window_pos_; + std::pair window_size_; #ifdef __APPLE__ - // Workaround for perpertually broken OpenGL VSync on macOS, - // most recently https://github.com/glfw/glfw/issues/2249. - std::optional core_video_; + // Workaround for perpertually broken OpenGL VSync on macOS, + // most recently https://github.com/glfw/glfw/issues/2249. + std::optional core_video_; #endif -}; -} // namespace mujoco + }; +} // namespace mujoco -#endif // MUJOCO_SIMULATE_GLFW_ADAPTER_H_ +#endif // MUJOCO_SIMULATE_GLFW_ADAPTER_H_ diff --git a/simulate/src/mujoco/glfw_corevideo.h b/simulate/src/mujoco/glfw_corevideo.h index d855ff3..269eecd 100644 --- a/simulate/src/mujoco/glfw_corevideo.h +++ b/simulate/src/mujoco/glfw_corevideo.h @@ -28,30 +28,31 @@ #ifdef __OBJC__ #import #else -typedef void* CVDisplayLinkRef; +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(); +namespace mujoco +{ + class GlfwCoreVideo + { + public: + GlfwCoreVideo(GLFWwindow *window); + ~GlfwCoreVideo(); - void WaitForDisplayRefresh(); - int DisplayLinkCallback(); - void UpdateDisplayLink(); + void WaitForDisplayRefresh(); + int DisplayLinkCallback(); + void UpdateDisplayLink(); - private: - GLFWwindow* window_; - CVDisplayLinkRef display_link_; + private: + GLFWwindow *window_; + CVDisplayLinkRef display_link_; - std::atomic_bool waiting_; - std::mutex mu_; - std::condition_variable cond_; -}; -} // namespace mujoco + std::atomic_bool waiting_; + std::mutex mu_; + std::condition_variable cond_; + }; +} // namespace mujoco - -#endif // MUJOCO_SIMULATE_GLFW_COREVIDEO_H_ +#endif // MUJOCO_SIMULATE_GLFW_COREVIDEO_H_ diff --git a/simulate/src/mujoco/glfw_dispatch.cc b/simulate/src/mujoco/glfw_dispatch.cc index cc408e3..359440d 100644 --- a/simulate/src/mujoco/glfw_dispatch.cc +++ b/simulate/src/mujoco/glfw_dispatch.cc @@ -15,113 +15,121 @@ #include "glfw_dispatch.h" #ifdef mjGLFW_DYNAMIC_SYMBOLS - #ifdef _MSC_VER - #include - #include - #else - #include - #endif +#ifdef _MSC_VER +#include +#include +#else +#include +#endif #endif #include #include -namespace mujoco { +namespace mujoco +{ -// return dispatch table for glfw functions -const struct Glfw& Glfw(void* dlhandle) { + // return dispatch table for glfw functions + const struct Glfw &Glfw(void *dlhandle) { - // set static init_dlhandle - static const void* init_dlhandle = 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(); + // 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; + // make and intialize dispatch table + static const struct Glfw glfw = [&]() { // create and call constructor + // allocate + struct Glfw glfw; - // load glfw dynamically + // 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( \ - GetProcAddress(reinterpret_cast(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(dlsym(dlhandle, #func)) - #endif +#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( \ + GetProcAddress(reinterpret_cast(dlhandle), #func)) #else - #define mjGLFW_RESOLVE_SYMBOL(func) glfw.func = &::func + 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(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(); \ - } + // 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 + // 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); + mjGLFW_INITIALIZE_SYMBOL(glfwGetNSGLContext); #endif #undef mjGLFW_INITIALIZE_SYMBOL + return glfw; + }(); return glfw; - }(); - return glfw; -} -} // namespace mujoco + } +} // namespace mujoco diff --git a/simulate/src/mujoco/glfw_dispatch.h b/simulate/src/mujoco/glfw_dispatch.h index f3bec97..051ef98 100644 --- a/simulate/src/mujoco/glfw_dispatch.h +++ b/simulate/src/mujoco/glfw_dispatch.h @@ -22,56 +22,58 @@ #include #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 { +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 + // 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); + mjGLFW_DECLARE_SYMBOL(glfwGetNSGLContext); #endif #undef mjGLFW_DECLARE_SYMBOL -}; + }; -const struct Glfw& Glfw(void* dlhandle = nullptr); -} // namespace mujoco + const struct Glfw &Glfw(void *dlhandle = nullptr); +} // namespace mujoco -#endif // MUJOCO_SIMULATE_GLFW_DISPATCH_H_ +#endif // MUJOCO_SIMULATE_GLFW_DISPATCH_H_ diff --git a/simulate/src/mujoco/platform_ui_adapter.cc b/simulate/src/mujoco/platform_ui_adapter.cc index e1b5673..11a6b34 100644 --- a/simulate/src/mujoco/platform_ui_adapter.cc +++ b/simulate/src/mujoco/platform_ui_adapter.cc @@ -16,244 +16,285 @@ #include -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); - if(act==GLFW_PRESS) +namespace mujoco +{ + PlatformUIAdapter::PlatformUIAdapter() { - key_7_pressed_=(key==GLFW_KEY_7); - key_8_pressed_=(key==GLFW_KEY_8); - key_9_pressed_=(key==GLFW_KEY_9); + mjr_defaultContext(&con_); } - else + + void PlatformUIAdapter::FreeMjrContext() { - key_7_pressed_=false; - key_8_pressed_=false; - key_9_pressed_=false; - } - // release: nothing to do - if (!IsKeyDownEvent(act)) { - return; + mjr_freeContext(&con_); } - // update state - UpdateMjuiState(); - - // set key info - state_.type = mjEVENT_KEY; - state_.key = mj_key; - state_.keytime = std::chrono::duration( - std::chrono::steady_clock::now().time_since_epoch()).count(); - - // application-specific processing - if (event_callback_) { - event_callback_(&state_); + 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; } - last_key_ = mj_key; -} + bool PlatformUIAdapter::EnsureContextSize() + { + return false; + } -void PlatformUIAdapter::OnMouseButton(int button, int act) { - // translate API-specific mouse button code - mjtButton mj_button = TranslateMouseButton(button); + void PlatformUIAdapter::OnFilesDrop(int count, const char **paths) + { + state_.type = mjEVENT_FILESDROP; + state_.dropcount = count; + state_.droppaths = paths; - // update state - UpdateMjuiState(); + // application-specific processing + if (event_callback_) + { + event_callback_(&state_); + } - // 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; + // 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); + + if (act == GLFW_PRESS) + { + key_7_pressed_ = (key == GLFW_KEY_7); + key_8_pressed_ = (key == GLFW_KEY_8); + key_9_pressed_ = (key == GLFW_KEY_9); + } + else + { + key_7_pressed_ = false; + key_8_pressed_ = false; + key_9_pressed_ = false; + } + + // 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( + 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( + 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; } } - // press - if (IsMouseButtonDownEvent(act)) { - double now = std::chrono::duration( - 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; + void PlatformUIAdapter::OnMouseMove(double x, double y) + { + // no buttons down: nothing to do + if (!state_.left && !state_.right && !state_.middle) + { + return; } - // set info - state_.type = mjEVENT_PRESS; - state_.button = mj_button; - state_.buttontime = now; + // update state + UpdateMjuiState(); - // start dragging - if (state_.mouserect) { - state_.dragbutton = state_.button; - state_.dragrect = state_.mouserect; + // set move info + state_.type = mjEVENT_MOVE; + + // application-specific processing + if (event_callback_) + { + event_callback_(&state_); } } - // release - else { - state_.type = mjEVENT_RELEASE; + 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(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_); + } } - // 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_); + } } - // stop dragging after application processing - if (state_.type == mjEVENT_RELEASE) { - state_.dragrect = 0; + 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; -void PlatformUIAdapter::OnMouseMove(double x, double y) { - // no buttons down: nothing to do - if (!state_.left && !state_.right && !state_.middle) { - return; + // application-specific processing + if (event_callback_) + { + event_callback_(&state_); + } } - // update state - UpdateMjuiState(); + void PlatformUIAdapter::UpdateMjuiState() + { + // mouse buttons + state_.left = IsLeftMouseButtonPressed(); + state_.right = IsRightMouseButtonPressed(); + state_.middle = IsMiddleMouseButtonPressed(); - // set move info - state_.type = mjEVENT_MOVE; + // keyboard modifiers + state_.control = IsCtrlKeyPressed(); + state_.shift = IsShiftKeyPressed(); + state_.alt = IsAltKeyPressed(); - // application-specific processing - if (event_callback_) { - event_callback_(&state_); + // 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(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; } -} - -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(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(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 +} // namespace mujoco diff --git a/simulate/src/mujoco/platform_ui_adapter.h b/simulate/src/mujoco/platform_ui_adapter.h index 82d77ea..325690c 100644 --- a/simulate/src/mujoco/platform_ui_adapter.h +++ b/simulate/src/mujoco/platform_ui_adapter.h @@ -18,7 +18,6 @@ #include #include #include - #include namespace mujoco @@ -71,8 +70,6 @@ namespace mujoco virtual bool IsCtrlKeyPressed() const = 0; virtual bool IsShiftKeyPressed() const = 0; - virtual bool IsKeyPressed(int key) const = 0; - virtual bool IsMouseButtonDownEvent(int act) const = 0; virtual bool IsKeyDownEvent(int act) const = 0; diff --git a/simulate/src/mujoco/simulate.cc b/simulate/src/mujoco/simulate.cc index b88843c..1cc179a 100644 --- a/simulate/src/mujoco/simulate.cc +++ b/simulate/src/mujoco/simulate.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -26,7 +27,7 @@ #include #include -// #include "lodepng.h" +//#include "lodepng.h" #include #include #include @@ -132,7 +133,7 @@ namespace // file section of UI const mjuiDef defFile[] = { - {mjITEM_SECTION, "File", 1, nullptr, "AF"}, + {mjITEM_SECTION, "File", mjPRESERVE, nullptr, "AF"}, {mjITEM_BUTTON, "Save xml", 2, nullptr, ""}, {mjITEM_BUTTON, "Save mjb", 2, nullptr, ""}, {mjITEM_BUTTON, "Print model", 2, nullptr, "CM"}, @@ -651,13 +652,13 @@ namespace // prepare info text mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory"); mju::sprintf_arr(content, - "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%s\n%.2g of %s", + "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%s\n%.1f%% of %s", d->time, d->nefc, d->ncon, sim->run ? d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) : d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number), solerr, solver_niter, fps, - d->maxuse_arena / (double)(d->narena), + 100 * d->maxuse_arena / (double)(d->narena), mju_writeNumBytes(d->narena)); // add Energy if enabled @@ -728,16 +729,16 @@ namespace //---------------------------------- UI construction ----------------------------------------------- // make physics section of UI - void MakePhysicsSection(mj::Simulate *sim, int oldstate) + void MakePhysicsSection(mj::Simulate *sim) { mjOption *opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt; mjuiDef defPhysics[] = { - {mjITEM_SECTION, "Physics", oldstate, nullptr, "AP"}, + {mjITEM_SECTION, "Physics", mjPRESERVE, nullptr, "AP"}, {mjITEM_SELECT, "Integrator", 2, &(opt->integrator), "Euler\nRK4\nimplicit\nimplicitfast"}, {mjITEM_SELECT, "Cone", 2, &(opt->cone), "Pyramidal\nElliptic"}, {mjITEM_SELECT, "Jacobian", 2, &(opt->jacobian), "Dense\nSparse\nAuto"}, {mjITEM_SELECT, "Solver", 2, &(opt->solver), "PGS\nCG\nNewton"}, - {mjITEM_SEPARATOR, "Algorithmic Parameters", 1}, + {mjITEM_SEPARATOR, "Algorithmic Parameters", mjPRESERVE}, {mjITEM_EDITNUM, "Timestep", 2, &(opt->timestep), "1 0 1"}, {mjITEM_EDITINT, "Iterations", 2, &(opt->iterations), "1 0 1000"}, {mjITEM_EDITNUM, "Tolerance", 2, &(opt->tolerance), "1 0 1"}, @@ -745,38 +746,38 @@ namespace {mjITEM_EDITNUM, "LS Tol", 2, &(opt->ls_tolerance), "1 0 0.1"}, {mjITEM_EDITINT, "Noslip Iter", 2, &(opt->noslip_iterations), "1 0 1000"}, {mjITEM_EDITNUM, "Noslip Tol", 2, &(opt->noslip_tolerance), "1 0 1"}, - {mjITEM_EDITINT, "MPR Iter", 2, &(opt->mpr_iterations), "1 0 1000"}, - {mjITEM_EDITNUM, "MPR Tol", 2, &(opt->mpr_tolerance), "1 0 1"}, + {mjITEM_EDITINT, "CCD Iter", 2, &(opt->ccd_iterations), "1 0 1000"}, + {mjITEM_EDITNUM, "CCD Tol", 2, &(opt->ccd_tolerance), "1 0 1"}, {mjITEM_EDITNUM, "API Rate", 2, &(opt->apirate), "1 0 1000"}, {mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"}, {mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"}, - {mjITEM_SEPARATOR, "Physical Parameters", 1}, + {mjITEM_SEPARATOR, "Physical Parameters", mjPRESERVE}, {mjITEM_EDITNUM, "Gravity", 2, opt->gravity, "3"}, {mjITEM_EDITNUM, "Wind", 2, opt->wind, "3"}, {mjITEM_EDITNUM, "Magnetic", 2, opt->magnetic, "3"}, {mjITEM_EDITNUM, "Density", 2, &(opt->density), "1"}, {mjITEM_EDITNUM, "Viscosity", 2, &(opt->viscosity), "1"}, {mjITEM_EDITNUM, "Imp Ratio", 2, &(opt->impratio), "1"}, - {mjITEM_SEPARATOR, "Disable Flags", 1}, + {mjITEM_SEPARATOR, "Disable Flags", mjPRESERVE}, {mjITEM_END}}; mjuiDef defEnableFlags[] = { - {mjITEM_SEPARATOR, "Enable Flags", 1}, + {mjITEM_SEPARATOR, "Enable Flags", mjPRESERVE}, {mjITEM_END}}; mjuiDef defOverride[] = { - {mjITEM_SEPARATOR, "Contact Override", 1}, + {mjITEM_SEPARATOR, "Contact Override", mjPRESERVE}, {mjITEM_EDITNUM, "Margin", 2, &(opt->o_margin), "1"}, {mjITEM_EDITNUM, "Sol Imp", 2, &(opt->o_solimp), "5"}, {mjITEM_EDITNUM, "Sol Ref", 2, &(opt->o_solref), "2"}, {mjITEM_EDITNUM, "Friction", 2, &(opt->o_friction), "5"}, {mjITEM_END}}; mjuiDef defDisableActuator[] = { - {mjITEM_SEPARATOR, "Actuator Group Enable", 1}, - {mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator + 0, " 0"}, - {mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator + 1, " 1"}, - {mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator + 2, " 2"}, - {mjITEM_CHECKBYTE, "Act Group 3", 2, sim->enableactuator + 3, " 3"}, - {mjITEM_CHECKBYTE, "Act Group 4", 2, sim->enableactuator + 4, " 4"}, - {mjITEM_CHECKBYTE, "Act Group 5", 2, sim->enableactuator + 5, " 5"}, + {mjITEM_SEPARATOR, "Actuator Group Enable", mjPRESERVE}, + {mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator + 0, ""}, + {mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator + 1, ""}, + {mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator + 2, ""}, + {mjITEM_CHECKBYTE, "Act Group 3", 2, sim->enableactuator + 3, ""}, + {mjITEM_CHECKBYTE, "Act Group 4", 2, sim->enableactuator + 4, ""}, + {mjITEM_CHECKBYTE, "Act Group 5", 2, sim->enableactuator + 5, ""}, {mjITEM_END}}; // add physics @@ -804,41 +805,38 @@ namespace // add actuator group enable/disable mjui_add(&sim->ui0, defDisableActuator); + + // make some subsections closed by default + for (int i = 0; i < sim->ui0.sect[SECT_PHYSICS].nitem; i++) + { + mjuiItem *it = sim->ui0.sect[SECT_PHYSICS].item + i; + + // close less useful subsections + if (it->type == mjITEM_SEPARATOR) + { + if (mju::strcmp_arr(it->name, "Actuator Group Enable") && + mju::strcmp_arr(it->name, "Contact Override") && + mju::strcmp_arr(it->name, "Physical Parameters")) + { + it->state = mjSEPCLOSED + 1; + } + } + } } // make rendering section of UI - void MakeRenderingSection(mj::Simulate *sim, const mjModel *m, int oldstate) + void MakeRenderingSection(mj::Simulate *sim, const mjModel *m) { mjuiDef defRendering[] = { - {mjITEM_SECTION, - "Rendering", - oldstate, - nullptr, - "AR"}, - {mjITEM_SELECT, - "Camera", - 2, - &(sim->camera), - "Free\nTracking"}, - {mjITEM_SELECT, - "Label", - 2, - &(sim->opt.label), + {mjITEM_SECTION, "Rendering", mjPRESERVE, nullptr, "AR"}, + {mjITEM_SELECT, "Camera", 2, &(sim->camera), "Free\nTracking"}, + {mjITEM_SELECT, "Label", 2, &(sim->opt.label), "None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n" "Actuator\nConstraint\nFlex\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland"}, - {mjITEM_SELECT, - "Frame", - 2, - &(sim->opt.frame), + {mjITEM_SELECT, "Frame", 2, &(sim->opt.frame), "None\nBody\nGeom\nSite\nCamera\nLight\nContact\nWorld"}, - {mjITEM_BUTTON, - "Copy camera", - 2, - nullptr, - ""}, - {mjITEM_SEPARATOR, - "Model Elements", - 1}, + {mjITEM_BUTTON, "Copy camera", 2, nullptr, ""}, + {mjITEM_SEPARATOR, "Model Elements", 1}, {mjITEM_END}}; mjuiDef defOpenGL[] = { {mjITEM_SEPARATOR, "OpenGL Effects", 1}, @@ -877,17 +875,8 @@ namespace {mjITEM_END}}; for (int i = 0; i < mjNVISFLAG; i++) { - // set name, remove "&" + // set name mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]); - for (int j = 0; j < strlen(mjVISSTRING[i][0]); j++) - { - if (mjVISSTRING[i][0][j] == '&') - { - mju_strncpy( - defFlag[0].name + j, mjVISSTRING[i][0] + j + 1, mju::sizeof_arr(defFlag[0].name) - j); - break; - } - } // set shortcut and data if (mjVISSTRING[i][2][0]) @@ -913,7 +902,10 @@ namespace mjui_add(&sim->ui0, defOpenGL); for (int i = 0; i < mjNRNDFLAG; i++) { + // set name mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]); + + // set shortcut and data if (mjRNDSTRING[i][2][0]) { mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]); @@ -928,27 +920,29 @@ namespace } // make visualization section of UI - void MakeVisualizationSection(mj::Simulate *sim, const mjModel *m, int oldstate) + void MakeVisualizationSection(mj::Simulate *sim, const mjModel *m) { mjStatistic *stat = sim->is_passive_ ? &sim->scnstate_.model.stat : &sim->m_->stat; mjVisual *vis = sim->is_passive_ ? &sim->scnstate_.model.vis : &sim->m_->vis; mjuiDef defVisualization[] = { - {mjITEM_SECTION, "Visualization", oldstate, nullptr, "AV"}, + {mjITEM_SECTION, "Visualization", mjPRESERVE, nullptr, "AV"}, {mjITEM_SEPARATOR, "Headlight", 1}, {mjITEM_RADIO, "Active", 5, &(vis->headlight.active), "Off\nOn"}, {mjITEM_EDITFLOAT, "Ambient", 2, &(vis->headlight.ambient), "3"}, {mjITEM_EDITFLOAT, "Diffuse", 2, &(vis->headlight.diffuse), "3"}, {mjITEM_EDITFLOAT, "Specular", 2, &(vis->headlight.specular), "3"}, - {mjITEM_SEPARATOR, "Initial Free Camera", 1}, + {mjITEM_SEPARATOR, "Free Camera", 1}, + {mjITEM_RADIO, "Orthographic", 2, &(vis->global.orthographic), "No\nYes"}, + {mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"}, {mjITEM_EDITNUM, "Center", 2, &(stat->center), "3"}, {mjITEM_EDITFLOAT, "Azimuth", 2, &(vis->global.azimuth), "1"}, {mjITEM_EDITFLOAT, "Elevation", 2, &(vis->global.elevation), "1"}, {mjITEM_BUTTON, "Align", 2, nullptr, "CA"}, {mjITEM_SEPARATOR, "Global", 1}, {mjITEM_EDITNUM, "Extent", 2, &(stat->extent), "1"}, - {mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"}, {mjITEM_RADIO, "Inertia", 5, &(vis->global.ellipsoidinertia), "Box\nEllipsoid"}, + {mjITEM_RADIO, "BVH active", 5, &(vis->global.bvactive), "False\nTrue"}, {mjITEM_SEPARATOR, "Map", 1}, {mjITEM_EDITFLOAT, "Stiffness", 2, &(vis->map.stiffness), "1"}, {mjITEM_EDITFLOAT, "Rot stiffness", 2, &(vis->map.stiffnessrot), "1"}, @@ -962,8 +956,8 @@ namespace {mjITEM_EDITFLOAT, "Haze", 2, &(vis->map.haze), "1"}, {mjITEM_EDITFLOAT, "Shadow clip", 2, &(vis->map.shadowclip), "1"}, {mjITEM_EDITFLOAT, "Shadow scale", 2, &(vis->map.shadowscale), "1"}, - {mjITEM_SEPARATOR, "Scale", 1}, - {mjITEM_EDITNUM, "All [meansize]", 2, &(stat->meansize), "1"}, + {mjITEM_SEPARATOR, "Scale", mjPRESERVE}, + {mjITEM_EDITNUM, "All (meansize)", 2, &(stat->meansize), "1"}, {mjITEM_EDITFLOAT, "Force width", 2, &(vis->scale.forcewidth), "1"}, {mjITEM_EDITFLOAT, "Contact width", 2, &(vis->scale.contactwidth), "1"}, {mjITEM_EDITFLOAT, "Contact height", 2, &(vis->scale.contactheight), "1"}, @@ -980,17 +974,43 @@ namespace {mjITEM_EDITFLOAT, "Frame width", 2, &(vis->scale.framewidth), "1"}, {mjITEM_EDITFLOAT, "Constraint", 2, &(vis->scale.constraint), "1"}, {mjITEM_EDITFLOAT, "Slider-crank", 2, &(vis->scale.slidercrank), "1"}, + {mjITEM_SEPARATOR, "RGBA", mjPRESERVE}, + {mjITEM_EDITFLOAT, "fog", 2, &(vis->rgba.fog), "4"}, + {mjITEM_EDITFLOAT, "haze", 2, &(vis->rgba.haze), "4"}, + {mjITEM_EDITFLOAT, "force", 2, &(vis->rgba.force), "4"}, + {mjITEM_EDITFLOAT, "inertia", 2, &(vis->rgba.inertia), "4"}, + {mjITEM_EDITFLOAT, "joint", 2, &(vis->rgba.joint), "4"}, + {mjITEM_EDITFLOAT, "actuator", 2, &(vis->rgba.actuator), "4"}, + {mjITEM_EDITFLOAT, "actnegative", 2, &(vis->rgba.actuatornegative), "4"}, + {mjITEM_EDITFLOAT, "actpositive", 2, &(vis->rgba.actuatorpositive), "4"}, + {mjITEM_EDITFLOAT, "com", 2, &(vis->rgba.com), "4"}, + {mjITEM_EDITFLOAT, "camera", 2, &(vis->rgba.camera), "4"}, + {mjITEM_EDITFLOAT, "light", 2, &(vis->rgba.light), "4"}, + {mjITEM_EDITFLOAT, "selectpoint", 2, &(vis->rgba.selectpoint), "4"}, + {mjITEM_EDITFLOAT, "connect", 2, &(vis->rgba.connect), "4"}, + {mjITEM_EDITFLOAT, "contactpoint", 2, &(vis->rgba.contactpoint), "4"}, + {mjITEM_EDITFLOAT, "contactforce", 2, &(vis->rgba.contactforce), "4"}, + {mjITEM_EDITFLOAT, "contactfriction", 2, &(vis->rgba.contactfriction), "4"}, + {mjITEM_EDITFLOAT, "contacttorque", 2, &(vis->rgba.contacttorque), "4"}, + {mjITEM_EDITFLOAT, "contactgap", 2, &(vis->rgba.contactgap), "4"}, + {mjITEM_EDITFLOAT, "rangefinder", 2, &(vis->rgba.rangefinder), "4"}, + {mjITEM_EDITFLOAT, "constraint", 2, &(vis->rgba.constraint), "4"}, + {mjITEM_EDITFLOAT, "slidercrank", 2, &(vis->rgba.slidercrank), "4"}, + {mjITEM_EDITFLOAT, "crankbroken", 2, &(vis->rgba.crankbroken), "4"}, + {mjITEM_EDITFLOAT, "frustum", 2, &(vis->rgba.frustum), "4"}, + {mjITEM_EDITFLOAT, "bv", 2, &(vis->rgba.bv), "4"}, + {mjITEM_EDITFLOAT, "bvactive", 2, &(vis->rgba.bvactive), "4"}, {mjITEM_END}}; - // add rendering standard + // add visualization section mjui_add(&sim->ui0, defVisualization); } // make group section of UI - void MakeGroupSection(mj::Simulate *sim, int oldstate) + void MakeGroupSection(mj::Simulate *sim) { mjuiDef defGroup[] = { - {mjITEM_SECTION, "Group enable", oldstate, nullptr, "AG"}, + {mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"}, {mjITEM_SEPARATOR, "Geom groups", 1}, {mjITEM_CHECKBYTE, "Geom 0", 2, sim->opt.geomgroup, " 0"}, {mjITEM_CHECKBYTE, "Geom 1", 2, sim->opt.geomgroup + 1, " 1"}, @@ -1047,10 +1067,10 @@ namespace } // make joint section of UI - void MakeJointSection(mj::Simulate *sim, int oldstate) + void MakeJointSection(mj::Simulate *sim) { mjuiDef defJoint[] = { - {mjITEM_SECTION, "Joint", oldstate, nullptr, "AJ"}, + {mjITEM_SECTION, "Joint", mjPRESERVE, nullptr, "AJ"}, {mjITEM_END}}; mjuiDef defSlider[] = { {mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"}, @@ -1111,10 +1131,10 @@ namespace } // make control section of UI - void MakeControlSection(mj::Simulate *sim, int oldstate) + void MakeControlSection(mj::Simulate *sim) { mjuiDef defControl[] = { - {mjITEM_SECTION, "Control", oldstate, nullptr, "AC"}, + {mjITEM_SECTION, "Control", mjPRESERVE, nullptr, "AC"}, {mjITEM_BUTTON, "Clear all", 2}, {mjITEM_END}}; mjuiDef defSlider[] = { @@ -1180,39 +1200,17 @@ namespace // make model-dependent UI sections void MakeUiSections(mj::Simulate *sim, const mjModel *m, const mjData *d) { - // get section open-close state, UI 0 - int oldstate0[NSECT0]; - for (int i = 0; i < NSECT0; i++) - { - oldstate0[i] = 0; - if (sim->ui0.nsect > i) - { - oldstate0[i] = sim->ui0.sect[i].state; - } - } - - // get section open-close state, UI 1 - int oldstate1[NSECT1]; - for (int i = 0; i < NSECT1; i++) - { - oldstate1[i] = 0; - if (sim->ui1.nsect > i) - { - oldstate1[i] = sim->ui1.sect[i].state; - } - } - // clear model-dependent sections of UI sim->ui0.nsect = SECT_PHYSICS; sim->ui1.nsect = 0; // make - MakePhysicsSection(sim, oldstate0[SECT_PHYSICS]); - MakeRenderingSection(sim, m, oldstate0[SECT_RENDERING]); - MakeVisualizationSection(sim, m, oldstate0[SECT_VISUALIZATION]); - MakeGroupSection(sim, oldstate0[SECT_GROUP]); - MakeJointSection(sim, oldstate1[SECT_JOINT]); - MakeControlSection(sim, oldstate1[SECT_CONTROL]); + MakePhysicsSection(sim); + MakeRenderingSection(sim, m); + MakeVisualizationSection(sim, m); + MakeGroupSection(sim); + MakeJointSection(sim); + MakeControlSection(sim); } //---------------------------------- utility functions --------------------------------------------- @@ -1245,7 +1243,9 @@ namespace // millisecond timer, for MuJoCo built-in profiler mjtNum Timer() { - return Milliseconds(mj::Simulate::Clock::now().time_since_epoch()).count(); + static auto start = mj::Simulate::Clock::now(); + auto elapsed = Milliseconds(mj::Simulate::Clock::now() - start); + return elapsed.count(); } // clear all times @@ -1383,7 +1383,7 @@ namespace return sim->m_ || sim->is_passive_; case 3: // require model and nkey - return !sim->is_passive_ && sim->nkey_; + return (sim->m_ || sim->is_passive_) && sim->nkey_; case 4: // require model and paused return sim->m_ && !sim->run; @@ -1424,10 +1424,24 @@ namespace rect[3].height = rect[0].height; } + // modify UI void UiModify(mjUI *ui, mjuiState *state, mjrContext *con) { mjui_resize(ui, con); - mjr_addAux(ui->auxid, ui->width, ui->maxheight, ui->spacing.samples, con); + + // remake aux buffer only if missing or different + int id = ui->auxid; + if (con->auxFBO[id] == 0 || + con->auxFBO_r[id] == 0 || + con->auxColor[id] == 0 || + con->auxColor_r[id] == 0 || + con->auxWidth[id] != ui->width || + con->auxHeight[id] != ui->maxheight || + con->auxSamples[id] != ui->spacing.samples) + { + mjr_addAux(id, ui->width, ui->maxheight, ui->spacing.samples, con); + } + UiLayout(state); mjui_update(-1, -1, ui, state, con); } @@ -1648,7 +1662,7 @@ namespace if (it->name[0] == 'J' && it->name[1] == 'o') { sim->ui1.nsect = SECT_JOINT; - MakeJointSection(sim, sim->ui1.sect[SECT_JOINT].state); + MakeJointSection(sim); sim->ui1.nsect = NSECT1; UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); } @@ -1723,10 +1737,11 @@ namespace mjui0_update_section(sim, SECT_SIMULATION); } - // not in scrubber: step + // not in scrubber: step, add to history buffer else { mj_step(sim->m_, sim->d_); + sim->AddToHistory(); } UpdateProfiler(sim, sim->m_, sim->d_); @@ -2027,6 +2042,10 @@ namespace mujoco { return; } + if (this->exitrequest.load()) + { + return; + } bool update_profiler = this->profiler && (this->pause_update || this->run); bool update_sensor = this->sensor && (this->pause_update || this->run); @@ -2108,7 +2127,7 @@ namespace mujoco X(impratio); X(tolerance); X(noslip_tolerance); - X(mpr_tolerance); + X(ccd_tolerance); X(gravity); X(wind); X(magnetic); @@ -2124,7 +2143,7 @@ namespace mujoco X(solver); X(iterations); X(noslip_iterations); - X(mpr_iterations); + X(ccd_iterations); X(disableflags); X(enableflags); X(disableactuator); @@ -2182,6 +2201,7 @@ namespace mujoco { mj_resetData(m_, d_); mj_forward(m_, d_); + load_error[0] = '\0'; update_profiler = true; update_sensor = true; scrub_index = 0; @@ -2211,14 +2231,7 @@ namespace mujoco if (pending_.load_key) { - int i = this->key; - d_->time = m_->key_time[i]; - mju_copy(d_->qpos, m_->key_qpos + i * m_->nq, m_->nq); - mju_copy(d_->qvel, m_->key_qvel + i * m_->nv, m_->nv); - mju_copy(d_->act, m_->key_act + i * m_->na, m_->na); - mju_copy(d_->mocap_pos, m_->key_mpos + i * 3 * m_->nmocap, 3 * m_->nmocap); - mju_copy(d_->mocap_quat, m_->key_mquat + i * 4 * m_->nmocap, 4 * m_->nmocap); - mju_copy(d_->ctrl, m_->key_ctrl + i * m_->nu, m_->nu); + mj_resetDataKeyframe(m_, d_, this->key); mj_forward(m_, d_); update_profiler = true; update_sensor = true; @@ -2227,14 +2240,7 @@ namespace mujoco if (pending_.save_key) { - int i = this->key; - m_->key_time[i] = d_->time; - mju_copy(m_->key_qpos + i * m_->nq, d_->qpos, m_->nq); - mju_copy(m_->key_qvel + i * m_->nv, d_->qvel, m_->nv); - mju_copy(m_->key_act + i * m_->na, d_->act, m_->na); - mju_copy(m_->key_mpos + i * 3 * m_->nmocap, d_->mocap_pos, 3 * m_->nmocap); - mju_copy(m_->key_mquat + i * 4 * m_->nmocap, d_->mocap_quat, 4 * m_->nmocap); - mju_copy(m_->key_ctrl + i * m_->nu, d_->ctrl, m_->nu); + mj_setKeyframe(m_, d_, this->key); pending_.save_key = false; } @@ -2355,6 +2361,21 @@ namespace mujoco } } + // pick up rendering flags changed via user_scn + if (user_scn) + { + for (int i = 0; i < mjNRNDFLAG; ++i) + { + if (user_scn->flags[i] != user_scn_flags_prev_[i]) + { + scn.flags[i] = user_scn->flags[i]; + pending_.ui_update_rendering = true; + } + } + Copy(user_scn->flags, scn.flags); + Copy(user_scn_flags_prev_, user_scn->flags); + } + mjopt_prev_ = scnstate_.model.opt; warn_vgeomfull_prev_ = scnstate_.data.warning[mjWARN_VGEOMFULL].number; } @@ -2518,13 +2539,13 @@ namespace mujoco // allocate history buffer: smaller of {2000 states, 100 MB} if (!this->is_passive_) { - constexpr int kHistoryLength = 2000; constexpr int kMaxHistoryBytes = 1e8; // get state size, size of history buffer state_size_ = mj_stateSize(this->m_, mjSTATE_INTEGRATION); int state_bytes = state_size_ * sizeof(mjtNum); - int history_bytes = mjMIN(state_bytes * kHistoryLength, kMaxHistoryBytes); + int history_length = mjMIN(INT_MAX / state_bytes, 2000); + int history_bytes = mjMIN(state_bytes * history_length, kMaxHistoryBytes); nhistory_ = history_bytes / state_bytes; // allocate history buffer, reset cursor and UI slider @@ -2562,6 +2583,12 @@ namespace mujoco this->scn.flags[mjRND_REFLECTION] = 0; } + if (this->user_scn) + { + Copy(this->user_scn->flags, this->scn.flags); + Copy(this->user_scn_flags_prev_, this->scn.flags); + } + // clear perturbation state this->pert.active = 0; this->pert.select = 0; @@ -2773,7 +2800,7 @@ namespace mujoco if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { this->ui1.nsect = SECT_CONTROL; - MakeControlSection(this, this->ui1.sect[SECT_CONTROL].state); + MakeControlSection(this); this->ui1.nsect = NSECT1; UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); } @@ -2802,7 +2829,19 @@ namespace mujoco // show pause/loading label if (!this->run || this->loadrequest) { - const char *label = this->loadrequest ? "LOADING..." : "PAUSE"; + char label[30] = {'\0'}; + if (this->loadrequest) + { + std::snprintf(label, sizeof(label), "LOADING..."); + } + else if (this->scrub_index == 0) + { + std::snprintf(label, sizeof(label), "PAUSE"); + } + else + { + std::snprintf(label, sizeof(label), "PAUSE (%d)", this->scrub_index); + } mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, label, nullptr, &this->platform_ui->mjr_context()); } @@ -2900,10 +2939,14 @@ namespace mujoco // file dialog does not automatically open that location. Thus, we defer to a default // "screenshot.png" for now. // const std::string path = GetSavePath("screenshot.png"); - // if (!path.empty()) { - // if (lodepng::encode(path, rgb.get(), w, h, LCT_RGB)) { + // if (!path.empty()) + // { + // if (lodepng::encode(path, rgb.get(), w, h, LCT_RGB)) + // { // mju_error("could not save screenshot"); - // } else { + // } + // else + // { // std::printf("saved screenshot: %s\n", path.c_str()); // } // } @@ -2971,12 +3014,15 @@ namespace mujoco this->platform_ui->SetEventCallback(UiEvent); this->platform_ui->SetLayoutCallback(UiLayout); - // populate uis with standard sections + // populate uis with standard sections, open some sections initially this->ui0.userdata = this; this->ui1.userdata = this; mjui_add(&this->ui0, defFile); mjui_add(&this->ui0, this->def_option); mjui_add(&this->ui0, this->def_simulation); + this->ui0.sect[0].state = 1; + this->ui0.sect[1].state = 1; + this->ui0.sect[2].state = 1; mjui_add(&this->ui0, this->def_watch); UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); @@ -3061,7 +3107,7 @@ namespace mujoco this->platform_ui->key_9_pressed_ = false; } } - + // render while simulation is running this->Render(); @@ -3077,11 +3123,9 @@ namespace mujoco } } - if (!is_passive_) - { - mjv_freeScene(&this->scn); - } - else + const MutexLock lock(this->mtx); + mjv_freeScene(&this->scn); + if (is_passive_) { mjv_freeSceneState(&scnstate_); } @@ -3105,6 +3149,44 @@ namespace mujoco mj_getState(m_, d_, state, mjSTATE_INTEGRATION); } + // inject Brownian noise + void Simulate::InjectNoise() + { + // no noise, return + if (ctrl_noise_std <= 0) + { + return; + } + + // convert rate and scale to discrete time (Ornstein–Uhlenbeck) + mjtNum rate = mju_exp(-m_->opt.timestep / ctrl_noise_rate); + mjtNum scale = ctrl_noise_std * mju_sqrt(1 - rate * rate); + + for (int i = 0; i < m_->nu; i++) + { + mjtNum bottom = 0, top = 0, midpoint = 0, halfrange = 1; + if (m_->actuator_ctrllimited[i]) + { + bottom = m_->actuator_ctrlrange[2 * i]; + top = m_->actuator_ctrlrange[2 * i + 1]; + midpoint = 0.5 * (top + bottom); // target of exponential decay + halfrange = 0.5 * (top - bottom); // scales noise + } + + // exponential convergence to midpoint at ctrl_noise_rate + d_->ctrl[i] = rate * d_->ctrl[i] + (1 - rate) * midpoint; + + // add noise + d_->ctrl[i] += scale * halfrange * mju_standardNormal(nullptr); + + // clip to range if limited + if (m_->actuator_ctrllimited[i]) + { + d_->ctrl[i] = mju_clip(d_->ctrl[i], bottom, top); + } + } + } + void Simulate::UpdateHField(int hfieldid) { MutexLock lock(this->mtx); @@ -3140,9 +3222,8 @@ namespace mujoco cond_upload_.wait(lock, [this]() { return texture_upload_ == -1; }); } - - ElasticBand::ElasticBand(){}; - ElasticBand::~ElasticBand(){}; + ElasticBand::ElasticBand() {}; + ElasticBand::~ElasticBand() {}; void ElasticBand::Advance(std::vector x, std::vector dx) { diff --git a/simulate/src/mujoco/simulate.h b/simulate/src/mujoco/simulate.h index 08eda38..f0df39d 100644 --- a/simulate/src/mujoco/simulate.h +++ b/simulate/src/mujoco/simulate.h @@ -25,8 +25,6 @@ #include #include #include -#include -#include #include #include @@ -48,7 +46,6 @@ namespace mujoco bool enable_ = true; std::vector f_ = {0, 0, 0}; }; - // 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 @@ -106,6 +103,9 @@ namespace mujoco // add state to history buffer void AddToHistory(); + // inject control noise + void InjectNoise(); + // constants static constexpr int kMaxFilenameLength = 1000; @@ -150,6 +150,7 @@ namespace mujoco mjOption mjopt_prev_; mjvOption opt_prev_; mjvCamera cam_prev_; + int warn_vgeomfull_prev_; // pending GUI-driven actions, to be applied at the next call to Sync @@ -267,6 +268,7 @@ namespace mujoco // additional user-defined visualization geoms (used in passive mode) mjvScene *user_scn = nullptr; + mjtByte user_scn_flags_prev_[mjNRNDFLAG]; // OpenGL rendering and UI int refresh_rate = 60; @@ -280,7 +282,7 @@ namespace mujoco // 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", 1, nullptr, "AO"}, + {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"}, @@ -300,7 +302,7 @@ namespace mujoco // simulation section of UI const mjuiDef def_simulation[14] = { - {mjITEM_SECTION, "Simulation", 1, nullptr, "AS"}, + {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"}, @@ -309,15 +311,15 @@ namespace mujoco {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 2"}, - {mjITEM_SLIDERNUM, "Noise rate", 5, &this->ctrl_noise_rate, "0 2"}, + {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", 0, nullptr, "AW"}, + {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, " "}, @@ -336,7 +338,6 @@ namespace mujoco ElasticBand elastic_band_; int use_elastic_band_ = 0; }; - } // namespace mujoco #endif