From 25d44d5fecbbcfcb3aca72388e22cfc2d5576890 Mon Sep 17 00:00:00 2001 From: lgv Date: Tue, 30 Jun 2026 16:03:00 +0800 Subject: [PATCH] refactor(mujoco): split world and viewer runtime --- .../devices/mujoco/mujoco_viewer.pb.txt | 7 + .../config/devices/mujoco/mujoco_world.pb.txt | 7 + cmvr-es/simulate/mujoco/CMakeLists.txt | 3 +- .../mujoco/mujoco_viewer/CMakeLists.txt | 42 +- .../mujoco_viewer/include/array_safety.h | 105 - .../mujoco_viewer/include/glfw_adapter.h | 78 - .../mujoco_viewer/include/glfw_corevideo.h | 57 - .../mujoco_viewer/include/glfw_dispatch.h | 77 - .../mujoco/mujoco_viewer/include/lodepng.h | 2173 ------------ .../mujoco_viewer/include/mujoco_viewer.h | 94 +- .../include/platform_ui_adapter.h | 101 - .../mujoco/mujoco_viewer/include/simulate.h | 353 -- .../mujoco/mujoco_viewer/src/glfw_adapter.cc | 252 -- .../mujoco/mujoco_viewer/src/glfw_dispatch.cc | 127 - .../mujoco_viewer/src/mujoco_viewer.cpp | 548 +-- .../mujoco_viewer/src/mujoco_viewer_test.cpp | 303 +- .../mujoco_viewer/src/platform_ui_adapter.cc | 247 -- .../mujoco/mujoco_viewer/src/simulate.cc | 2982 ----------------- .../mujoco/mujoco_world/CMakeLists.txt | 15 + .../mujoco_world/include/mujoco_world.h | 148 + .../mujoco/mujoco_world/src/mujoco_world.cpp | 776 +++++ cmvr-es/test/CMakeLists.txt | 17 + cmvr-es/test/mujoco_manual_ui_test.cpp | 237 ++ .../mujoco_config/mujoco_world_config.proto | 27 + 24 files changed, 1673 insertions(+), 7103 deletions(-) create mode 100644 cmvr-es/config/devices/mujoco/mujoco_viewer.pb.txt create mode 100644 cmvr-es/config/devices/mujoco/mujoco_world.pb.txt delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/array_safety.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_adapter.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_corevideo.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/lodepng.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/include/simulate.h delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_adapter.cc delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_dispatch.cc delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/src/platform_ui_adapter.cc delete mode 100644 cmvr-es/simulate/mujoco/mujoco_viewer/src/simulate.cc create mode 100644 cmvr-es/simulate/mujoco/mujoco_world/CMakeLists.txt create mode 100644 cmvr-es/simulate/mujoco/mujoco_world/include/mujoco_world.h create mode 100644 cmvr-es/simulate/mujoco/mujoco_world/src/mujoco_world.cpp create mode 100644 cmvr-es/test/CMakeLists.txt create mode 100644 cmvr-es/test/mujoco_manual_ui_test.cpp create mode 100644 protos/cmvr/config/mujoco_config/mujoco_world_config.proto diff --git a/cmvr-es/config/devices/mujoco/mujoco_viewer.pb.txt b/cmvr-es/config/devices/mujoco/mujoco_viewer.pb.txt new file mode 100644 index 00000000..4ca51ac7 --- /dev/null +++ b/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 +} diff --git a/cmvr-es/config/devices/mujoco/mujoco_world.pb.txt b/cmvr-es/config/devices/mujoco/mujoco_world.pb.txt new file mode 100644 index 00000000..5bde8886 --- /dev/null +++ b/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 +} diff --git a/cmvr-es/simulate/mujoco/CMakeLists.txt b/cmvr-es/simulate/mujoco/CMakeLists.txt index 66999041..ba1a5e90 100644 --- a/cmvr-es/simulate/mujoco/CMakeLists.txt +++ b/cmvr-es/simulate/mujoco/CMakeLists.txt @@ -1 +1,2 @@ -add_subdirectory(mujoco_viewer) \ No newline at end of file +add_subdirectory(mujoco_world) +add_subdirectory(mujoco_viewer) diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt b/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt index 962d6abc..54ce1b93 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt @@ -1,14 +1,15 @@ -file(GLOB SRC - ${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cc +add_library(mujoco_viewer SHARED + src/mujoco_viewer.cpp ) -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) -#find_package(Python3 COMPONENTS NumPy) -# -# -#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 -#) - +add_executable(mujoco_viewer_test + ${CMAKE_CURRENT_SOURCE_DIR}/src/mujoco_viewer_test.cpp +) +target_link_libraries(mujoco_viewer_test + PRIVATE + gtest + gtest_main + cmvr_es::mujoco_viewer + cmvr_es::mujoco_world +) diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/array_safety.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/array_safety.h deleted file mode 100644 index e8dd0d73..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/array_safety.h +++ /dev/null @@ -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 -#include -#include -#include -#include - -// 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 -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) { - { - 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/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_adapter.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_adapter.h deleted file mode 100644 index 1490c333..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_adapter.h +++ /dev/null @@ -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 - -#include -#include -#include "platform_ui_adapter.h" - -#ifdef __APPLE__ -#include -#include "glfw_corevideo.h" -#endif - -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; - - 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 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_; -#endif -}; -} // namespace mujoco - -#endif // MUJOCO_SIMULATE_GLFW_ADAPTER_H_ diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_corevideo.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_corevideo.h deleted file mode 100644 index d855ff31..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_corevideo.h +++ /dev/null @@ -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 -#include -#include - -#include "glfw_dispatch.h" - -#ifdef __OBJC__ -#import -#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_ diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h deleted file mode 100644 index f3bec97e..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/glfw_dispatch.h +++ /dev/null @@ -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 - -#ifdef __APPLE__ -#define GLFW_EXPOSE_NATIVE_NSGL -#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 { -#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_ diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/lodepng.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/lodepng.h deleted file mode 100644 index c1570219..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/lodepng.h +++ /dev/null @@ -1,2173 +0,0 @@ -/* -LodePNG version 20250506 - -Copyright (c) 2005-2025 Lode Vandevenne - -This software is provided 'as-is', without any express or implied -warranty. In no event will the authors be held liable for any damages -arising from the use of this software. - -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - - 3. This notice may not be removed or altered from any source - distribution. -*/ - -#ifndef LODEPNG_H -#define LODEPNG_H - -#include /*for size_t*/ - -extern const char* LODEPNG_VERSION_STRING; - -/* -The following #defines are used to create code sections. They can be disabled -to disable code sections, which can give faster compile time and smaller binary. -The "NO_COMPILE" defines are designed to be used to pass as defines to the -compiler command to disable them without modifying this header, e.g. --DLODEPNG_NO_COMPILE_ZLIB for gcc or clang. -*/ -/*deflate & zlib. If disabled, you must specify alternative zlib functions in -the custom_zlib field of the compress and decompress settings*/ -#ifndef LODEPNG_NO_COMPILE_ZLIB -/*pass -DLODEPNG_NO_COMPILE_ZLIB to the compiler to disable this, or comment out LODEPNG_COMPILE_ZLIB below*/ -#define LODEPNG_COMPILE_ZLIB -#endif - -/*png encoder and png decoder*/ -#ifndef LODEPNG_NO_COMPILE_PNG -/*pass -DLODEPNG_NO_COMPILE_PNG to the compiler to disable this, or comment out LODEPNG_COMPILE_PNG below*/ -#define LODEPNG_COMPILE_PNG -#endif - -/*deflate&zlib decoder and png decoder*/ -#ifndef LODEPNG_NO_COMPILE_DECODER -/*pass -DLODEPNG_NO_COMPILE_DECODER to the compiler to disable this, or comment out LODEPNG_COMPILE_DECODER below*/ -#define LODEPNG_COMPILE_DECODER -#endif - -/*deflate&zlib encoder and png encoder*/ -#ifndef LODEPNG_NO_COMPILE_ENCODER -/*pass -DLODEPNG_NO_COMPILE_ENCODER to the compiler to disable this, or comment out LODEPNG_COMPILE_ENCODER below*/ -#define LODEPNG_COMPILE_ENCODER -#endif - -/*the optional built in harddisk file loading and saving functions*/ -#ifndef LODEPNG_NO_COMPILE_DISK -/*pass -DLODEPNG_NO_COMPILE_DISK to the compiler to disable this, or comment out LODEPNG_COMPILE_DISK below*/ -#define LODEPNG_COMPILE_DISK -#endif - -/*support for chunks other than IHDR, IDAT, PLTE, tRNS, IEND: ancillary and unknown chunks*/ -#ifndef LODEPNG_NO_COMPILE_ANCILLARY_CHUNKS -/*pass -DLODEPNG_NO_COMPILE_ANCILLARY_CHUNKS to the compiler to disable this, -or comment out LODEPNG_COMPILE_ANCILLARY_CHUNKS below*/ -#define LODEPNG_COMPILE_ANCILLARY_CHUNKS -#endif - -/*ability to convert error numerical codes to English text string*/ -#ifndef LODEPNG_NO_COMPILE_ERROR_TEXT -/*pass -DLODEPNG_NO_COMPILE_ERROR_TEXT to the compiler to disable this, -or comment out LODEPNG_COMPILE_ERROR_TEXT below*/ -#define LODEPNG_COMPILE_ERROR_TEXT -#endif - -/*Compile the default allocators (C's free, malloc and realloc). If you disable this, -you can define the functions lodepng_free, lodepng_malloc and lodepng_realloc in your -source files with custom allocators.*/ -#ifndef LODEPNG_NO_COMPILE_ALLOCATORS -/*pass -DLODEPNG_NO_COMPILE_ALLOCATORS to the compiler to disable the built-in ones, -or comment out LODEPNG_COMPILE_ALLOCATORS below*/ -#define LODEPNG_COMPILE_ALLOCATORS -#endif - -/*Disable built-in CRC function, in that case a custom implementation of -lodepng_crc32 must be defined externally so that it can be linked in. -The default built-in CRC code comes with 8KB of lookup tables, so for memory constrained environment you may want it -disabled and provide a much smaller implementation externally as said above. You can find such an example implementation -in a comment in the lodepng.c(pp) file in the 'else' case of the searchable LODEPNG_COMPILE_CRC section.*/ -#ifndef LODEPNG_NO_COMPILE_CRC -/*pass -DLODEPNG_NO_COMPILE_CRC to the compiler to disable the built-in one, -or comment out LODEPNG_COMPILE_CRC below*/ -#define LODEPNG_COMPILE_CRC -#endif - -/*compile the C++ version (you can disable the C++ wrapper here even when compiling for C++)*/ -#ifdef __cplusplus -#ifndef LODEPNG_NO_COMPILE_CPP -/*pass -DLODEPNG_NO_COMPILE_CPP to the compiler to disable C++ (not needed if a C-only compiler), -or comment out LODEPNG_COMPILE_CPP below*/ -#define LODEPNG_COMPILE_CPP -#endif -#endif - -#ifdef LODEPNG_COMPILE_CPP -#include -#include -#endif /*LODEPNG_COMPILE_CPP*/ - -#ifdef LODEPNG_COMPILE_PNG -/*The PNG color types (also used for raw image).*/ -typedef enum LodePNGColorType { - LCT_GREY = 0, /*grayscale: 1,2,4,8,16 bit*/ - LCT_RGB = 2, /*RGB: 8,16 bit*/ - LCT_PALETTE = 3, /*palette: 1,2,4,8 bit*/ - LCT_GREY_ALPHA = 4, /*grayscale with alpha: 8,16 bit*/ - LCT_RGBA = 6, /*RGB with alpha: 8,16 bit*/ - /*LCT_MAX_OCTET_VALUE lets the compiler allow this enum to represent any invalid - byte value from 0 to 255 that could be present in an invalid PNG file header. Do - not use, compare with or set the name LCT_MAX_OCTET_VALUE, instead either use - the valid color type names above, or numeric values like 1 or 7 when checking for - particular disallowed color type byte values, or cast to integer to print it.*/ - LCT_MAX_OCTET_VALUE = 255 -} LodePNGColorType; - -#ifdef LODEPNG_COMPILE_DECODER -/* -Converts PNG data in memory to raw pixel data. -out: Output parameter. Pointer to buffer that will contain the raw pixel data. - After decoding, its size is w * h * (bytes per pixel) bytes larger than - initially. Bytes per pixel depends on colortype and bitdepth. - Must be freed after usage with free(*out). - Note: for 16-bit per channel colors, uses big endian format like PNG does. -w: Output parameter. Pointer to width of pixel data. -h: Output parameter. Pointer to height of pixel data. -in: Memory buffer with the PNG file. -insize: size of the in buffer. -colortype: the desired color type for the raw output image. See explanation on PNG color types. -bitdepth: the desired bit depth for the raw output image. See explanation on PNG color types. -Return value: LodePNG error code (0 means no error). -*/ -unsigned lodepng_decode_memory(unsigned char** out, unsigned* w, unsigned* h, - const unsigned char* in, size_t insize, - LodePNGColorType colortype, unsigned bitdepth); - -/*Same as lodepng_decode_memory, but always decodes to 32-bit RGBA raw image*/ -unsigned lodepng_decode32(unsigned char** out, unsigned* w, unsigned* h, - const unsigned char* in, size_t insize); - -/*Same as lodepng_decode_memory, but always decodes to 24-bit RGB raw image*/ -unsigned lodepng_decode24(unsigned char** out, unsigned* w, unsigned* h, - const unsigned char* in, size_t insize); - -#ifdef LODEPNG_COMPILE_DISK -/* -Load PNG from disk, from file with given name. -Same as the other decode functions, but instead takes a filename as input. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory.*/ -unsigned lodepng_decode_file(unsigned char** out, unsigned* w, unsigned* h, - const char* filename, - LodePNGColorType colortype, unsigned bitdepth); - -/*Same as lodepng_decode_file, but always decodes to 32-bit RGBA raw image. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory.*/ -unsigned lodepng_decode32_file(unsigned char** out, unsigned* w, unsigned* h, - const char* filename); - -/*Same as lodepng_decode_file, but always decodes to 24-bit RGB raw image. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory.*/ -unsigned lodepng_decode24_file(unsigned char** out, unsigned* w, unsigned* h, - const char* filename); -#endif /*LODEPNG_COMPILE_DISK*/ -#endif /*LODEPNG_COMPILE_DECODER*/ - - -#ifdef LODEPNG_COMPILE_ENCODER -/* -Converts raw pixel data into a PNG image in memory. The colortype and bitdepth - of the output PNG image cannot be chosen, they are automatically determined - by the colortype, bitdepth and content of the input pixel data. - Note: for 16-bit per channel colors, needs big endian format like PNG does. -out: Output parameter. Pointer to buffer that will contain the PNG image data. - Must be freed after usage with free(*out). -outsize: Output parameter. Pointer to the size in bytes of the out buffer. -image: The raw pixel data to encode. The size of this buffer should be - w * h * (bytes per pixel), bytes per pixel depends on colortype and bitdepth. -w: width of the raw pixel data in pixels. -h: height of the raw pixel data in pixels. -colortype: the color type of the raw input image. See explanation on PNG color types. -bitdepth: the bit depth of the raw input image. See explanation on PNG color types. -Return value: LodePNG error code (0 means no error). -*/ -unsigned lodepng_encode_memory(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h, - LodePNGColorType colortype, unsigned bitdepth); - -/*Same as lodepng_encode_memory, but always encodes from 32-bit RGBA raw image.*/ -unsigned lodepng_encode32(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h); - -/*Same as lodepng_encode_memory, but always encodes from 24-bit RGB raw image.*/ -unsigned lodepng_encode24(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h); - -#ifdef LODEPNG_COMPILE_DISK -/* -Converts raw pixel data into a PNG file on disk. -Same as the other encode functions, but instead takes a filename as output. - -NOTE: This overwrites existing files without warning! - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and encode in-memory.*/ -unsigned lodepng_encode_file(const char* filename, - const unsigned char* image, unsigned w, unsigned h, - LodePNGColorType colortype, unsigned bitdepth); - -/*Same as lodepng_encode_file, but always encodes from 32-bit RGBA raw image. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and encode in-memory.*/ -unsigned lodepng_encode32_file(const char* filename, - const unsigned char* image, unsigned w, unsigned h); - -/*Same as lodepng_encode_file, but always encodes from 24-bit RGB raw image. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and encode in-memory.*/ -unsigned lodepng_encode24_file(const char* filename, - const unsigned char* image, unsigned w, unsigned h); -#endif /*LODEPNG_COMPILE_DISK*/ -#endif /*LODEPNG_COMPILE_ENCODER*/ - - -#ifdef LODEPNG_COMPILE_CPP -namespace lodepng { -#ifdef LODEPNG_COMPILE_DECODER -/*Same as lodepng_decode_memory, but decodes to an std::vector. The colortype -is the format to output the pixels to. Default is RGBA 8-bit per channel.*/ -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - const unsigned char* in, size_t insize, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - const std::vector& in, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -#ifdef LODEPNG_COMPILE_DISK -/* -Converts PNG file from disk to raw pixel data in memory. -Same as the other decode functions, but instead takes a filename as input. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory. -*/ -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - const std::string& filename, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -#endif /* LODEPNG_COMPILE_DISK */ -#endif /* LODEPNG_COMPILE_DECODER */ - -#ifdef LODEPNG_COMPILE_ENCODER -/*Same as lodepng_encode_memory, but encodes to an std::vector. colortype -is that of the raw input data. The output PNG color type will be auto chosen.*/ -unsigned encode(std::vector& out, - const unsigned char* in, unsigned w, unsigned h, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -unsigned encode(std::vector& out, - const std::vector& in, unsigned w, unsigned h, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -#ifdef LODEPNG_COMPILE_DISK -/* -Converts 32-bit RGBA raw pixel data into a PNG file on disk. -Same as the other encode functions, but instead takes a filename as output. - -NOTE: This overwrites existing files without warning! - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory. -*/ -unsigned encode(const std::string& filename, - const unsigned char* in, unsigned w, unsigned h, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -unsigned encode(const std::string& filename, - const std::vector& in, unsigned w, unsigned h, - LodePNGColorType colortype = LCT_RGBA, unsigned bitdepth = 8); -#endif /* LODEPNG_COMPILE_DISK */ -#endif /* LODEPNG_COMPILE_ENCODER */ -} /* namespace lodepng */ -#endif /*LODEPNG_COMPILE_CPP*/ -#endif /*LODEPNG_COMPILE_PNG*/ - -#ifdef LODEPNG_COMPILE_ERROR_TEXT -/*Returns an English description of the numerical error code.*/ -const char* lodepng_error_text(unsigned code); -#endif /*LODEPNG_COMPILE_ERROR_TEXT*/ - -#ifdef LODEPNG_COMPILE_DECODER -/*Settings for zlib decompression*/ -typedef struct LodePNGDecompressSettings LodePNGDecompressSettings; -struct LodePNGDecompressSettings { - /* Check LodePNGDecoderSettings for more ignorable errors such as ignore_crc */ - unsigned ignore_adler32; /*if 1, continue and don't give an error message if the Adler32 checksum is corrupted*/ - unsigned ignore_nlen; /*ignore complement of len checksum in uncompressed blocks*/ - - /*Maximum decompressed size, beyond this the decoder may (and is encouraged to) stop decoding, - return an error, output a data size > max_output_size and all the data up to that point. This is - not hard limit nor a guarantee, but can prevent excessive memory usage. This setting is - ignored by the PNG decoder, but is used by the deflate/zlib decoder and can be used by custom ones. - Set to 0 to impose no limit (the default).*/ - size_t max_output_size; - - /*use custom zlib decoder instead of built in one (default: null). - Should return 0 if success, any non-0 if error (numeric value not exposed).*/ - unsigned (*custom_zlib)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGDecompressSettings*); - /*use custom deflate decoder instead of built in one (default: null) - if custom_zlib is not null, custom_inflate is ignored (the zlib format uses deflate). - Should return 0 if success, any non-0 if error (numeric value not exposed).*/ - unsigned (*custom_inflate)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGDecompressSettings*); - - const void* custom_context; /*optional custom settings for custom functions*/ -}; - -extern const LodePNGDecompressSettings lodepng_default_decompress_settings; -void lodepng_decompress_settings_init(LodePNGDecompressSettings* settings); -#endif /*LODEPNG_COMPILE_DECODER*/ - -#ifdef LODEPNG_COMPILE_ENCODER -/* -Settings for zlib compression. Tweaking these settings tweaks the balance -between speed and compression ratio. -*/ -typedef struct LodePNGCompressSettings LodePNGCompressSettings; -struct LodePNGCompressSettings /*deflate = compress*/ { - /*LZ77 related settings*/ - unsigned btype; /*the block type for LZ (0, 1, 2 or 3, see zlib standard). Should be 2 for proper compression.*/ - unsigned use_lz77; /*whether or not to use LZ77. Should be 1 for proper compression.*/ - unsigned windowsize; /*must be a power of two <= 32768. higher compresses more but is slower. Default value: 2048.*/ - unsigned minmatch; /*minimum lz77 length. 3 is normally best, 6 can be better for some PNGs. Default: 0*/ - unsigned nicematch; /*stop searching if >= this length found. Set to 258 for best compression. Default: 128*/ - unsigned lazymatching; /*use lazy matching: better compression but a bit slower. Default: true*/ - - /*use custom zlib encoder instead of built in one (default: null)*/ - unsigned (*custom_zlib)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGCompressSettings*); - /*use custom deflate encoder instead of built in one (default: null) - if custom_zlib is used, custom_deflate is ignored since only the built in - zlib function will call custom_deflate*/ - unsigned (*custom_deflate)(unsigned char**, size_t*, - const unsigned char*, size_t, - const LodePNGCompressSettings*); - - const void* custom_context; /*optional custom settings for custom functions*/ -}; - -extern const LodePNGCompressSettings lodepng_default_compress_settings; -void lodepng_compress_settings_init(LodePNGCompressSettings* settings); -#endif /*LODEPNG_COMPILE_ENCODER*/ - -#ifdef LODEPNG_COMPILE_PNG -/* -Color mode of an image. Contains all information required to decode the pixel -bits to RGBA colors. This information is the same as used in the PNG file -format, and is used both for PNG and raw image data in LodePNG. -*/ -typedef struct LodePNGColorMode { - /*header (IHDR)*/ - LodePNGColorType colortype; /*color type, see PNG standard or documentation further in this header file*/ - unsigned bitdepth; /*bits per sample, see PNG standard or documentation further in this header file*/ - - /* - palette (PLTE and tRNS) - - Dynamically allocated with the colors of the palette, including alpha. - This field may not be allocated directly, use lodepng_color_mode_init first, - then lodepng_palette_add per color to correctly initialize it (to ensure size - of exactly 1024 bytes). - - The alpha channels must be set as well, set them to 255 for opaque images. - - When decoding, with the default settings you can ignore this palette, since - LodePNG already fills the palette colors in the pixels of the raw RGBA output, - but when decoding to the original PNG color mode it is needed to reconstruct - the colors. - - The palette is only supported for color type 3. - */ - unsigned char* palette; /*palette in RGBARGBA... order. Must be either 0, or when allocated must have 1024 bytes*/ - size_t palettesize; /*palette size in number of colors (amount of used bytes is 4 * palettesize)*/ - - /* - transparent color key (tRNS) - - This color uses the same bit depth as the bitdepth value in this struct, which can be 1-bit to 16-bit. - For grayscale PNGs, r, g and b will all 3 be set to the same. - - When decoding, by default you can ignore this information, since LodePNG sets - pixels with this key to transparent already in the raw RGBA output. - - The color key is only supported for color types 0 and 2. - */ - unsigned key_defined; /*is a transparent color key given? 0 = false, 1 = true*/ - unsigned key_r; /*red/grayscale component of color key*/ - unsigned key_g; /*green component of color key*/ - unsigned key_b; /*blue component of color key*/ -} LodePNGColorMode; - -/*init, cleanup and copy functions to use with this struct*/ -void lodepng_color_mode_init(LodePNGColorMode* info); -void lodepng_color_mode_cleanup(LodePNGColorMode* info); -/*return value is error code (0 means no error)*/ -unsigned lodepng_color_mode_copy(LodePNGColorMode* dest, const LodePNGColorMode* source); -/* Makes a temporary LodePNGColorMode that does not need cleanup (no palette) */ -LodePNGColorMode lodepng_color_mode_make(LodePNGColorType colortype, unsigned bitdepth); - -void lodepng_palette_clear(LodePNGColorMode* info); -/*add 1 color to the palette*/ -unsigned lodepng_palette_add(LodePNGColorMode* info, - unsigned char r, unsigned char g, unsigned char b, unsigned char a); - -/*get the total amount of bits per pixel, based on colortype and bitdepth in the struct*/ -unsigned lodepng_get_bpp(const LodePNGColorMode* info); -/*get the amount of color channels used, based on colortype in the struct. -If a palette is used, it counts as 1 channel.*/ -unsigned lodepng_get_channels(const LodePNGColorMode* info); -/*is it a grayscale type? (only colortype 0 or 4)*/ -unsigned lodepng_is_greyscale_type(const LodePNGColorMode* info); -/*has it got an alpha channel? (only colortype 2 or 6)*/ -unsigned lodepng_is_alpha_type(const LodePNGColorMode* info); -/*has it got a palette? (only colortype 3)*/ -unsigned lodepng_is_palette_type(const LodePNGColorMode* info); -/*only returns true if there is a palette and there is a value in the palette with alpha < 255. -Loops through the palette to check this.*/ -unsigned lodepng_has_palette_alpha(const LodePNGColorMode* info); -/* -Check if the given color info indicates the possibility of having non-opaque pixels in the PNG image. -Returns true if the image can have translucent or invisible pixels (it still be opaque if it doesn't use such pixels). -Returns false if the image can only have opaque pixels. -In detail, it returns true only if it's a color type with alpha, or has a palette with non-opaque values, -or if "key_defined" is true. -*/ -unsigned lodepng_can_have_alpha(const LodePNGColorMode* info); -/*Returns the byte size of a raw image buffer with given width, height and color mode*/ -size_t lodepng_get_raw_size(unsigned w, unsigned h, const LodePNGColorMode* color); - -#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS -/*The information of a Time chunk in PNG.*/ -typedef struct LodePNGTime { - unsigned year; /*2 bytes used (0-65535)*/ - unsigned month; /*1-12*/ - unsigned day; /*1-31*/ - unsigned hour; /*0-23*/ - unsigned minute; /*0-59*/ - unsigned second; /*0-60 (to allow for leap seconds)*/ -} LodePNGTime; -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ - -/*Information about the PNG image, except pixels, width and height.*/ -typedef struct LodePNGInfo { - /*header (IHDR), palette (PLTE) and transparency (tRNS) chunks*/ - unsigned compression_method;/*compression method of the original file. Always 0.*/ - unsigned filter_method; /*filter method of the original file*/ - unsigned interlace_method; /*interlace method of the original file: 0=none, 1=Adam7*/ - LodePNGColorMode color; /*color type and bits, palette and transparency of the PNG file*/ - -#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS - /* - Suggested background color chunk (bKGD) - - This uses the same color mode and bit depth as the PNG (except no alpha channel), - with values truncated to the bit depth in the unsigned integer. - - For grayscale and palette PNGs, the value is stored in background_r. The values - in background_g and background_b are then unused. The decoder will set them - equal to background_r, the encoder ignores them in this case. - - When decoding, you may get these in a different color mode than the one you requested - for the raw pixels: the colortype and bitdepth defined by info_png.color, that is the - ones defined in the header of the PNG image, are used. - - When encoding with auto_convert, you must use the color model defined in info_png.color for - these values. The encoder normally ignores info_png.color when auto_convert is on, but will - use it to interpret these values (and convert copies of them to its chosen color model). - - When encoding, avoid setting this to an expensive color, such as a non-gray value - when the image is gray, or the compression will be worse since it will be forced to - write the PNG with a more expensive color mode (when auto_convert is on). - - The decoder does not use this background color to edit the color of pixels. This is a - completely optional metadata feature. - */ - unsigned background_defined; /*is a suggested background color given?*/ - unsigned background_r; /*red/gray/palette component of suggested background color*/ - unsigned background_g; /*green component of suggested background color*/ - unsigned background_b; /*blue component of suggested background color*/ - - /* - Non-international text chunks (tEXt and zTXt) - - The char** arrays each contain num strings. The actual messages are in - text_strings, while text_keys are keywords that give a short description what - the actual text represents, e.g. Title, Author, Description, or anything else. - - All the string fields below including strings, keys, names and language tags are null terminated. - The PNG specification uses null characters for the keys, names and tags, and forbids null - characters to appear in the main text which is why we can use null termination everywhere here. - - A keyword is minimum 1 character and maximum 79 characters long (plus the - additional null terminator). It's discouraged to use a single line length - longer than 79 characters for texts. - - Don't allocate these text buffers yourself. Use the init/cleanup functions - correctly and use lodepng_add_text and lodepng_clear_text. - - Standard text chunk keywords and strings are encoded using Latin-1. - */ - size_t text_num; /*the amount of texts in these char** buffers (there may be more texts in itext)*/ - char** text_keys; /*the keyword of a text chunk (e.g. "Comment")*/ - char** text_strings; /*the actual text*/ - - /* - International text chunks (iTXt) - Similar to the non-international text chunks, but with additional strings - "langtags" and "transkeys", and the following text encodings are used: - keys: Latin-1, langtags: ASCII, transkeys and strings: UTF-8. - keys must be 1-79 characters (plus the additional null terminator), the other - strings are any length. - */ - size_t itext_num; /*the amount of international texts in this PNG*/ - char** itext_keys; /*the English keyword of the text chunk (e.g. "Comment")*/ - char** itext_langtags; /*language tag for this text's language, ISO/IEC 646 string, e.g. ISO 639 language tag*/ - char** itext_transkeys; /*keyword translated to the international language - UTF-8 string*/ - char** itext_strings; /*the actual international text - UTF-8 string*/ - - /* - Optional exif metadata in exif_size bytes. - Don't allocate this buffer yourself. Use the init/cleanup functions - correctly and use lodepng_set_exif and lodepng_clear_exif. - The exif data is in exif-encoded form but without JPEG markers, starting with the 'II' or 'MM' marker that indicates - endianness. It's up to an exif handling library to encode/decode its information. - */ - unsigned exif_defined; /* Whether exif metadata is present, that is, the PNG image has an eXIf chunk */ - unsigned char* exif; /* The bytes of the exif metadata, if present */ - unsigned exif_size; /* The size of the exif data in bytes */ - - - /*time chunk (tIME)*/ - unsigned time_defined; /*set to 1 to make the encoder generate a tIME chunk*/ - LodePNGTime time; - - /*phys chunk (pHYs)*/ - unsigned phys_defined; /*if 0, there is no pHYs chunk and the values below are undefined, if 1 else there is one*/ - unsigned phys_x; /*pixels per unit in x direction*/ - unsigned phys_y; /*pixels per unit in y direction*/ - unsigned phys_unit; /*may be 0 (unknown unit) or 1 (metre)*/ - - /* - Color profile related chunk types: cICP, iCPP, sRGB, gAMA, cHRM, sBIT - - LodePNG does not apply any color conversions on pixels in the encoder or decoder and does not interpret these color - profile values. It merely passes on the information. If you wish to use color profiles and convert colors, a separate - color management library should be used. There is also a limited library for this in lodepng_util.h. - - There are 4 types of (sets of) chunks providing color information. If multiple are present, each will be decoded by - LodePNG, but only one should be handled by the user, with the following order of priority depending on what the user - supports: - 1: cICP: Coding-independent code points (CICP) - 2: iCCP: ICC profile - 3: sRGB: indicates the image is in the sRGB color profile - 4: gAMA and cHRM: indicates a gamma and chromaticity value to define the color profile - */ - - /* - gAMA chunk: Image gamma - Optional, overridden by cICP, iCCP or sRGB if those are present. - Together with cHRM, this is a primitive way of specifying the image color profile. - */ - unsigned gama_defined; /* Whether a gAMA chunk is present (0 = not present, 1 = present). */ - unsigned gama_gamma; /* Gamma exponent times 100000 */ - - /* - cHRM chunk: Primary chromaticities and white point - Optional, overridden by cICP, iCCP or sRGB if those are present. - Together with gAMA, this is a primitive way of specifying the image color profile. - */ - unsigned chrm_defined; /* Whether a cHRM chunk is present (0 = not present, 1 = present). */ - unsigned chrm_white_x; /* White Point x times 100000 */ - unsigned chrm_white_y; /* White Point y times 100000 */ - unsigned chrm_red_x; /* Red x times 100000 */ - unsigned chrm_red_y; /* Red y times 100000 */ - unsigned chrm_green_x; /* Green x times 100000 */ - unsigned chrm_green_y; /* Green y times 100000 */ - unsigned chrm_blue_x; /* Blue x times 100000 */ - unsigned chrm_blue_y; /* Blue y times 100000 */ - - /* - sRGB chunk: Indicates the image is in the sRGB color space. - Optional. Should not appear at the same time as iCCP. - If gAMA is also present gAMA must contain value 45455. - If cHRM is also present cHRM must contain respectively 31270,32900,64000,33000,30000,60000,15000,6000. - */ - unsigned srgb_defined; /* Whether an sRGB chunk is present (0 = not present, 1 = present). */ - unsigned srgb_intent; /* Rendering intent: 0=perceptual, 1=rel. colorimetric, 2=saturation, 3=abs. colorimetric */ - - /* - iCCP chunk: Embedded ICC profile. - Optional. Should not appear at the same time as sRGB. - - Contains ICC profile, which can use any version of the ICC.1 specification by the International Color Consortium. See - its specification for more details. LodePNG does not parse or use the ICC profile (except its color space header - field for "RGB" or "GRAY", see below), a separate library to handle the ICC data format is needed to use it for color - management and conversions. - - For encoding, if iCCP is present, the PNG specification recommends to also add gAMA and cHRM chunks that approximate - the ICC profile, for compatibility with applications that don't use the ICC chunk. This is not required, and it's up - to the user to compute approximate values and set then in the appropriate gama_ and chrm_ fields, LodePNG does not do - this automatically since it does not interpret the ICC profile. - - For encoding, the ICC profile is required by the PNG specification to be an "RGB" profile for non-gray PNG color - types (types 2, 3 and 6) and a "GRAY" profile for gray PNG color types (types 1 and 4). If you disable auto_convert, - you must ensure the ICC profile type matches your requested color type, else the encoder gives an error. If - auto_convert is enabled (the default), and the ICC profile is not a correct match for the pixel data, this will result - in an encoder error if the pixel data has non-gray pixels for a GRAY profile, or a silent less-optimal compression of - the pixel data if the pixels could be encoded as grayscale but the ICC profile is RGB. - - To avoid this do not set an ICC profile in the image unless there is a good reason for it, and when doing so - make sure you compute it carefully to avoid the above problems. - */ - unsigned iccp_defined; /* Whether an iCCP chunk is present (0 = not present, 1 = present). */ - char* iccp_name; /* Null terminated string with profile name, 1-79 bytes */ - /* - The ICC profile in iccp_profile_size bytes. - Don't allocate this buffer yourself. Use the init/cleanup functions - correctly and use lodepng_set_icc and lodepng_clear_icc. - */ - unsigned char* iccp_profile; - unsigned iccp_profile_size; /* The size of iccp_profile in bytes */ - - /* - cICP chunk: Coding-independent code points for video signal type identification. - Optional. If present, and supported, overrides iCCP, sRGB, gAMA and cHRM. - The meaning of the values are as defined in the specification ITU-T-H.273. LodePNG does not - use these values, only passes on the metadata. The meaning of the values is they are enum - values representing certain color spaces, including HDR color spaces, such as Display P3, - PQ and HLG. The video full range flag value should typically be 1 for the use cases of PNG - images, but can be 0 for narrow-range images in certain video editing workflows. - */ - unsigned cicp_defined; /* Whether an cICP chunk is present (0 = not present, 1 = present). */ - unsigned cicp_color_primaries; /* Colour primaries value */ - unsigned cicp_transfer_function; /* Transfer characteristics value */ - unsigned cicp_matrix_coefficients; /* Matrix coefficients value */ - unsigned cicp_video_full_range_flag; /* Video full range flag value */ - - /* - mDCV chunk: Mastering Display Color Volume. - Optional, typically used in conjunction with certain HDR color spaces that can - be represented by the cICP chunk. - See the PNG specification, third edition, for more information on this chunk. - All the red, green, blue and white x and y values are encoded as 16-bit - integers and therefore must be in range 0-65536. The min and max luminance - values are 32-bit integers. - */ - unsigned mdcv_defined; /* Whether an mDCV chunk is present (0 = not present, 1 = present). */ - /* Mastering display color primary chromaticities (CIE 1931 x,y of R,G,B) */ - unsigned mdcv_red_x; /* Red x times 50000 */ - unsigned mdcv_red_y; /* Red y times 50000 */ - unsigned mdcv_green_x; /* Green x times 50000 */ - unsigned mdcv_green_y; /* Green y times 50000 */ - unsigned mdcv_blue_x; /* Blue x times 50000 */ - unsigned mdcv_blue_y; /* Blue y times 50000 */ - /* Mastering display white point chromaticity (CIE 1931 x,y) */ - unsigned mdcv_white_x; /* White Point x times 50000 */ - unsigned mdcv_white_y; /* White Point y times 50000 */ - /* Mastering display luminance */ - unsigned mdcv_max_luminance; /* Max luminance in cd/m^2 times 10000 */ - unsigned mdcv_min_luminance; /* Min luminance in cd/m^2 times 10000 */ - - /* - cLLI chunk: Content Light Level Information. - Optional, typically used in conjunction with certain HDR color spaces that can - be represented by the cICP chunk. - See the PNG specification, third edition, for more information on this chunk. - The clli_max_cll and clli_max_fall values are 32-bit integers. - */ - unsigned clli_defined; /* Whether a cLLI chunk is present (0 = not present, 1 = present). */ - unsigned clli_max_cll; /* Maximum Content Light Level (MaxCLL) in cd/m^2 times 10000 */ - unsigned clli_max_fall; /* Maximum Frame-Average Light Level (MaxFALL) in cd/m^2 times 10000 */ - - /* - sBIT chunk: significant bits. - Optional metadata, only set this if needed. - - If defined, these values give the bit depth of the original data. Since PNG only stores 1, 2, 4, 8 or 16-bit - per channel data, the significant bits value can be used to indicate the original encoded data has another - sample depth, such as 10 or 12. - - Encoders using this value, when storing the pixel data, should use the most significant bits - of the data to store the original bits, and use a good sample depth scaling method such as - "left bit replication" to fill in the least significant bits, rather than fill zeroes. - - Decoders using this value, if able to work with data that's e.g. 10-bit or 12-bit, should right - shift the data to go back to the original bit depth, but decoders are also allowed to ignore - sbit and work e.g. with the 8-bit or 16-bit data from the PNG directly, since thanks - to the encoder contract, the values encoded in PNG are in valid range for the PNG bit depth. - - For grayscale images, sbit_g and sbit_b are not used, and for images that don't use color - type RGBA or grayscale+alpha, sbit_a is not used (it's not used even for palette images with - translucent palette values, or images with color key). The values that are used must be - greater than zero and smaller than or equal to the PNG bit depth. - - The color type from the header in the PNG image defines these used and unused fields: if - decoding with a color mode conversion, such as always decoding to RGBA, this metadata still - only uses the color type of the original PNG, and may e.g. lack the alpha channel info - if the PNG was RGB. When encoding with auto_convert (as well as without), also always the - color model defined in info_png.color determines this. - - NOTE: enabling sbit can hurt compression, because the encoder can then not always use - auto_convert to choose a more optimal color mode for the data, because the PNG format has - strict requirements for the allowed sbit values in combination with color modes. - For example, setting these fields to 10-bit will force the encoder to keep using a 16-bit per channel - color mode, even if the pixel data would in fact fit in a more efficient 8-bit mode. - */ - unsigned sbit_defined; /*is significant bits given? if not, the values below are unused*/ - unsigned sbit_r; /*red or gray component of significant bits*/ - unsigned sbit_g; /*green component of significant bits*/ - unsigned sbit_b; /*blue component of significant bits*/ - unsigned sbit_a; /*alpha component of significant bits*/ - - /* End of color profile related chunks */ - - - /* - unknown chunks: chunks not known by LodePNG, passed on byte for byte. - - There are 3 buffers, one for each position in the PNG where unknown chunks can appear. - Each buffer contains all unknown chunks for that position consecutively. - The 3 positions are: - 0: between IHDR and PLTE, 1: between PLTE and IDAT, 2: between IDAT and IEND. - - For encoding, do not store critical chunks or known chunks that are enabled with a "_defined" flag - above in here, since the encoder will blindly follow this and could then encode an invalid PNG file - (such as one with two IHDR chunks or the disallowed combination of sRGB with iCCP). But do use - this if you wish to store an ancillary chunk that is not supported by LodePNG (such as sPLT or hIST), - or any non-standard PNG chunk. - - Do not allocate or traverse this data yourself. Use the chunk traversing functions declared - later, such as lodepng_chunk_next and lodepng_chunk_append, to read/write this struct. - */ - unsigned char* unknown_chunks_data[3]; - size_t unknown_chunks_size[3]; /*size in bytes of the unknown chunks, given for protection*/ -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ -} LodePNGInfo; - -/*init, cleanup and copy functions to use with this struct*/ -void lodepng_info_init(LodePNGInfo* info); -void lodepng_info_cleanup(LodePNGInfo* info); -/*return value is error code (0 means no error)*/ -unsigned lodepng_info_copy(LodePNGInfo* dest, const LodePNGInfo* source); - -#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS -unsigned lodepng_add_text(LodePNGInfo* info, const char* key, const char* str); /*push back both texts at once*/ -void lodepng_clear_text(LodePNGInfo* info); /*use this to clear the texts again after you filled them in*/ - -unsigned lodepng_add_itext(LodePNGInfo* info, const char* key, const char* langtag, - const char* transkey, const char* str); /*push back the 4 texts of 1 chunk at once*/ -void lodepng_clear_itext(LodePNGInfo* info); /*use this to clear the itexts again after you filled them in*/ - -/*replaces if exists*/ -unsigned lodepng_set_icc(LodePNGInfo* info, const char* name, const unsigned char* profile, unsigned profile_size); -void lodepng_clear_icc(LodePNGInfo* info); /*use this to clear the profile again after you filled it in*/ - -/*replaces if exists*/ -unsigned lodepng_set_exif(LodePNGInfo* info, const unsigned char* exif, unsigned exif_size); -void lodepng_clear_exif(LodePNGInfo* info); /*use this to clear the exif metadata again after you filled it in*/ -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ - -/* -Converts raw buffer from one color type to another color type, based on -LodePNGColorMode structs to describe the input and output color type. -See the reference manual at the end of this header file to see which color conversions are supported. -return value = LodePNG error code (0 if all went ok, an error if the conversion isn't supported) -The out buffer must have size (w * h * bpp + 7) / 8, where bpp is the bits per pixel -of the output color type (lodepng_get_bpp). -For < 8 bpp images, there should not be padding bits at the end of scanlines. -For 16-bit per channel colors, uses big endian format like PNG does. -Return value is LodePNG error code -*/ -unsigned lodepng_convert(unsigned char* out, const unsigned char* in, - const LodePNGColorMode* mode_out, const LodePNGColorMode* mode_in, - unsigned w, unsigned h); - -#ifdef LODEPNG_COMPILE_DECODER -/* -Settings for the decoder. This contains settings for the PNG and the Zlib -decoder, but not the Info settings from the Info structs. -*/ -typedef struct LodePNGDecoderSettings { - LodePNGDecompressSettings zlibsettings; /*in here is the setting to ignore Adler32 checksums*/ - - /* Check LodePNGDecompressSettings for more ignorable errors such as ignore_adler32 */ - unsigned ignore_crc; /*ignore CRC checksums*/ - unsigned ignore_critical; /*ignore unknown critical chunks*/ - unsigned ignore_end; /*ignore issues at end of file if possible (missing IEND chunk, too large chunk, ...)*/ - /* TODO: make a system involving warnings with levels and a strict mode instead. Other potentially recoverable - errors: srgb rendering intent value, size of content of ancillary chunks, more than 79 characters for some - strings, placement/combination rules for ancillary chunks, crc of unknown chunks, allowed characters - in string keys, invalid characters in chunk types names, etc... */ - - unsigned color_convert; /*whether to convert the PNG to the color type you want. Default: yes*/ - -#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS - unsigned read_text_chunks; /*if false but remember_unknown_chunks is true, they're stored in the unknown chunks*/ - - /*store all bytes from unknown chunks in the LodePNGInfo (off by default, useful for a png editor)*/ - unsigned remember_unknown_chunks; - - /* maximum size for decompressed text chunks. If a text chunk's text is larger than this, an error is returned, - unless reading text chunks is disabled or this limit is set higher or disabled. Set to 0 to allow any size. - By default it is a value that prevents unreasonably large strings from hogging memory. */ - size_t max_text_size; - - /* maximum size for compressed ICC chunks. If the ICC profile is larger than this, an error will be returned. Set to - 0 to allow any size. By default this is a value that prevents ICC profiles that would be much larger than any - legitimate profile could be to hog memory. */ - size_t max_icc_size; -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ -} LodePNGDecoderSettings; - -void lodepng_decoder_settings_init(LodePNGDecoderSettings* settings); -#endif /*LODEPNG_COMPILE_DECODER*/ - -#ifdef LODEPNG_COMPILE_ENCODER -/*strategy to use to choose the PNG filter per scanline. Strategies 0-4 correspond -to each of the 5 filter types PNG supports, the next values are adaptive strategies*/ -typedef enum LodePNGFilterStrategy { - /*every filter at zero*/ - LFS_ZERO = 0, - /*every filter at 1, 2, 3 or 4 (paeth), unlike LFS_ZERO not a good choice, but for testing*/ - LFS_ONE = 1, - LFS_TWO = 2, - LFS_THREE = 3, - LFS_FOUR = 4, - /*Use the filter out of the 5 above types that gives minimum sum, by trying each one. This is the adaptive filtering - suggested heuristic in the PNG standard chapter 'Filter selection'.*/ - LFS_MINSUM, - /*Use the filter type that gives smallest Shannon entropy for this scanline. Depending - on the image, this is better or worse than minsum.*/ - LFS_ENTROPY, - /* - Brute-force-search PNG filters by compressing each filter for each scanline. - Experimental, very slow, and only rarely gives better compression than MINSUM. - */ - LFS_BRUTE_FORCE, - /*use predefined_filters buffer: you specify the filter type for each scanline*/ - LFS_PREDEFINED -} LodePNGFilterStrategy; - -/*Gives characteristics about the integer RGBA colors of the image (count, alpha channel usage, bit depth, ...), -which helps decide which color model to use for encoding. -Used internally by default if "auto_convert" is enabled. Public because it's useful for custom algorithms.*/ -typedef struct LodePNGColorStats { - unsigned colored; /*not grayscale*/ - unsigned key; /*image is not opaque and color key is possible instead of full alpha*/ - unsigned short key_r; /*key values, always as 16-bit, in 8-bit case the byte is duplicated, e.g. 65535 means 255*/ - unsigned short key_g; - unsigned short key_b; - unsigned alpha; /*image is not opaque and alpha channel or alpha palette required*/ - unsigned numcolors; /*amount of colors, up to 257. Not valid if bits == 16 or allow_palette is disabled.*/ - unsigned char palette[1024]; /*Remembers up to the first 256 RGBA colors, in no particular order, only valid when numcolors is valid*/ - unsigned bits; /*bits per channel (not for palette). 1,2 or 4 for grayscale only. 16 if 16-bit per channel required.*/ - size_t numpixels; - - /*user settings for computing/using the stats*/ - unsigned allow_palette; /*default 1. if 0, disallow choosing palette colortype in auto_choose_color, and don't count numcolors*/ - unsigned allow_greyscale; /*default 1. if 0, choose RGB or RGBA even if the image only has gray colors*/ -} LodePNGColorStats; - -void lodepng_color_stats_init(LodePNGColorStats* stats); - -/*Get a LodePNGColorStats of the image. The stats must already have been inited. -Returns error code (e.g. alloc fail) or 0 if ok.*/ -unsigned lodepng_compute_color_stats(LodePNGColorStats* stats, - const unsigned char* image, unsigned w, unsigned h, - const LodePNGColorMode* mode_in); - -/*Settings for the encoder.*/ -typedef struct LodePNGEncoderSettings { - LodePNGCompressSettings zlibsettings; /*settings for the zlib encoder, such as window size, ...*/ - - /*automatically choose output PNG color type. If false, must explicitely choose the output color - type in state.info_png.color.colortype, info_png.color.bitdepth and optionally its palette. - Default: true*/ - unsigned auto_convert; - - /*If true, follows the suggestion in the PNG standard in chapter 'Filter selection': if the PNG uses - a palette or lower than 8 bit depth, set all filters to zero. - In other cases this will use the heuristic from the chosen filter_strategy. The PNG standard - suggests LFS_MINSUM for those cases.*/ - unsigned filter_palette_zero; - /*Which filter strategy to use when not using zeroes due to filter_palette_zero. - Set filter_palette_zero to 0 to ensure always using your chosen strategy. Default: LFS_MINSUM*/ - LodePNGFilterStrategy filter_strategy; - /*used if filter_strategy is LFS_PREDEFINED. In that case, this must point to a buffer with - the same length as the amount of scanlines in the image, and each value must <= 5. You - have to cleanup this buffer, LodePNG will never free it. Don't forget that filter_palette_zero - must be set to 0 to ensure this is also used on palette or low bitdepth images.*/ - const unsigned char* predefined_filters; - - /*force creating a PLTE chunk if colortype is 2 or 6 (= a suggested palette). - If colortype is 3, PLTE is always created. If color type is explicitely set - to a grayscale type (1 or 4), this is not done and is ignored. If enabling this, - a palette must be present in the info_png. - NOTE: enabling this may worsen compression if auto_convert is used to choose - optimal color mode, because it cannot use grayscale color modes in this case*/ - unsigned force_palette; -#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS - /*add LodePNG identifier and version as a text chunk, for debugging*/ - unsigned add_id; - /*encode text chunks as zTXt chunks instead of tEXt chunks, and use compression in iTXt chunks*/ - unsigned text_compression; -#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/ -} LodePNGEncoderSettings; - -void lodepng_encoder_settings_init(LodePNGEncoderSettings* settings); -#endif /*LODEPNG_COMPILE_ENCODER*/ - - -#if defined(LODEPNG_COMPILE_DECODER) || defined(LODEPNG_COMPILE_ENCODER) -/*The settings, state and information for extended encoding and decoding.*/ -typedef struct LodePNGState { -#ifdef LODEPNG_COMPILE_DECODER - LodePNGDecoderSettings decoder; /*the decoding settings*/ -#endif /*LODEPNG_COMPILE_DECODER*/ -#ifdef LODEPNG_COMPILE_ENCODER - LodePNGEncoderSettings encoder; /*the encoding settings*/ -#endif /*LODEPNG_COMPILE_ENCODER*/ - LodePNGColorMode info_raw; /*specifies the format in which you would like to get the raw pixel buffer*/ - LodePNGInfo info_png; /*info of the PNG image obtained after decoding*/ - unsigned error; -} LodePNGState; - -/*init, cleanup and copy functions to use with this struct*/ -void lodepng_state_init(LodePNGState* state); -void lodepng_state_cleanup(LodePNGState* state); -void lodepng_state_copy(LodePNGState* dest, const LodePNGState* source); -#endif /* defined(LODEPNG_COMPILE_DECODER) || defined(LODEPNG_COMPILE_ENCODER) */ - -#ifdef LODEPNG_COMPILE_DECODER -/* -Same as lodepng_decode_memory, but uses a LodePNGState to allow custom settings and -getting much more information about the PNG image and color mode. -*/ -unsigned lodepng_decode(unsigned char** out, unsigned* w, unsigned* h, - LodePNGState* state, - const unsigned char* in, size_t insize); - -/* -Read the PNG header, but not the actual data. This returns only the information -that is in the IHDR chunk of the PNG, such as width, height and color type. The -information is placed in the info_png field of the LodePNGState. -*/ -unsigned lodepng_inspect(unsigned* w, unsigned* h, - LodePNGState* state, - const unsigned char* in, size_t insize); -#endif /*LODEPNG_COMPILE_DECODER*/ - -/* -Reads one metadata chunk (other than IHDR, which is handled by lodepng_inspect) -of the PNG file and outputs what it read in the state. Returns error code on failure. -Use lodepng_inspect first with a new state, then e.g. lodepng_chunk_find_const -to find the desired chunk type, and if non null use lodepng_inspect_chunk (with -chunk_pointer - start_of_file as pos). -Supports most metadata chunks from the PNG standard (gAMA, bKGD, tEXt, ...). -Ignores unsupported, unknown, non-metadata or IHDR chunks (without error). -Requirements: &in[pos] must point to start of a chunk, must use regular -lodepng_inspect first since format of most other chunks depends on IHDR, and if -there is a PLTE chunk, that one must be inspected before tRNS or bKGD. -*/ -unsigned lodepng_inspect_chunk(LodePNGState* state, size_t pos, - const unsigned char* in, size_t insize); - -#ifdef LODEPNG_COMPILE_ENCODER -/*This function allocates the out buffer with standard malloc and stores the size in *outsize.*/ -unsigned lodepng_encode(unsigned char** out, size_t* outsize, - const unsigned char* image, unsigned w, unsigned h, - LodePNGState* state); -#endif /*LODEPNG_COMPILE_ENCODER*/ - -/* -The lodepng_chunk functions are normally not needed, except to traverse the -unknown chunks stored in the LodePNGInfo struct, or add new ones to it. -It also allows traversing the chunks of an encoded PNG file yourself. - -The chunk pointer always points to the beginning of the chunk itself, that is -the first byte of the 4 length bytes. - -In the PNG file format, chunks have the following format: --4 bytes length: length of the data of the chunk in bytes (chunk itself is 12 bytes longer) --4 bytes chunk type (ASCII a-z,A-Z only, see below) --length bytes of data (may be 0 bytes if length was 0) --4 bytes of CRC, computed on chunk name + data - -The first chunk starts at the 8th byte of the PNG file, the entire rest of the file -exists out of concatenated chunks with the above format. - -PNG standard chunk ASCII naming conventions: --First byte: uppercase = critical, lowercase = ancillary --Second byte: uppercase = public, lowercase = private --Third byte: must be uppercase --Fourth byte: uppercase = unsafe to copy, lowercase = safe to copy -*/ - -/* -Gets the length of the data of the chunk. Total chunk length has 12 bytes more. -There must be at least 4 bytes to read from. If the result value is too large, -it may be corrupt data. -*/ -unsigned lodepng_chunk_length(const unsigned char* chunk); - -/*puts the 4-byte type in null terminated string*/ -void lodepng_chunk_type(char type[5], const unsigned char* chunk); - -/*check if the type is the given type*/ -unsigned char lodepng_chunk_type_equals(const unsigned char* chunk, const char* type); - -/*0: it's one of the critical chunk types, 1: it's an ancillary chunk (see PNG standard)*/ -unsigned char lodepng_chunk_ancillary(const unsigned char* chunk); - -/*0: public, 1: private (see PNG standard)*/ -unsigned char lodepng_chunk_private(const unsigned char* chunk); - -/*0: the chunk is unsafe to copy, 1: the chunk is safe to copy (see PNG standard)*/ -unsigned char lodepng_chunk_safetocopy(const unsigned char* chunk); - -/*get pointer to the data of the chunk, where the input points to the header of the chunk*/ -unsigned char* lodepng_chunk_data(unsigned char* chunk); -const unsigned char* lodepng_chunk_data_const(const unsigned char* chunk); - -/*returns 0 if the crc is correct, 1 if it's incorrect (0 for OK as usual!)*/ -unsigned lodepng_chunk_check_crc(const unsigned char* chunk); - -/*generates the correct CRC from the data and puts it in the last 4 bytes of the chunk*/ -void lodepng_chunk_generate_crc(unsigned char* chunk); - -/* -Iterate to next chunks, allows iterating through all chunks of the PNG file. -Input must be at the beginning of a chunk (result of a previous lodepng_chunk_next call, -or the 8th byte of a PNG file which always has the first chunk), or alternatively may -point to the first byte of the PNG file (which is not a chunk but the magic header, the -function will then skip over it and return the first real chunk). -Will output pointer to the start of the next chunk, or at or beyond end of the file if there -is no more chunk after this or possibly if the chunk is corrupt. -Start this process at the 8th byte of the PNG file. -In a non-corrupt PNG file, the last chunk should have name "IEND". -*/ -unsigned char* lodepng_chunk_next(unsigned char* chunk, unsigned char* end); -const unsigned char* lodepng_chunk_next_const(const unsigned char* chunk, const unsigned char* end); - -/*Finds the first chunk with the given type in the range [chunk, end), or returns NULL if not found.*/ -unsigned char* lodepng_chunk_find(unsigned char* chunk, unsigned char* end, const char type[5]); -const unsigned char* lodepng_chunk_find_const(const unsigned char* chunk, const unsigned char* end, const char type[5]); - -/* -Appends chunk to the data in out. The given chunk should already have its chunk header. -The out variable and outsize are updated to reflect the new reallocated buffer. -Returns error code (0 if it went ok) -*/ -unsigned lodepng_chunk_append(unsigned char** out, size_t* outsize, const unsigned char* chunk); - -/* -Appends new chunk to out. The chunk to append is given by giving its length, type -and data separately. The type is a 4-letter string. -The out variable and outsize are updated to reflect the new reallocated buffer. -Returne error code (0 if it went ok) -*/ -unsigned lodepng_chunk_create(unsigned char** out, size_t* outsize, size_t length, - const char* type, const unsigned char* data); - - -/*Calculate CRC32 of buffer*/ -unsigned lodepng_crc32(const unsigned char* buf, size_t len); -#endif /*LODEPNG_COMPILE_PNG*/ - - -#ifdef LODEPNG_COMPILE_ZLIB -/* -This zlib part can be used independently to zlib compress and decompress a -buffer. It cannot be used to create gzip files however, and it only supports the -part of zlib that is required for PNG, it does not support dictionaries. -*/ - -#ifdef LODEPNG_COMPILE_DECODER -/*Inflate a buffer. Inflate is the decompression step of deflate. Out buffer must be freed after use.*/ -unsigned lodepng_inflate(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGDecompressSettings* settings); - -/* -Decompresses Zlib data. Reallocates the out buffer and appends the data. The -data must be according to the zlib specification. -Either, *out must be NULL and *outsize must be 0, or, *out must be a valid -buffer and *outsize its size in bytes. out must be freed by user after usage. -*/ -unsigned lodepng_zlib_decompress(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGDecompressSettings* settings); -#endif /*LODEPNG_COMPILE_DECODER*/ - -#ifdef LODEPNG_COMPILE_ENCODER -/* -Compresses data with Zlib. Reallocates the out buffer and appends the data. -Zlib adds a small header and trailer around the deflate data. -The data is output in the format of the zlib specification. -Either, *out must be NULL and *outsize must be 0, or, *out must be a valid -buffer and *outsize its size in bytes. out must be freed by user after usage. -*/ -unsigned lodepng_zlib_compress(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGCompressSettings* settings); - -/* -Find length-limited Huffman code for given frequencies. This function is in the -public interface only for tests, it's used internally by lodepng_deflate. -*/ -unsigned lodepng_huffman_code_lengths(unsigned* lengths, const unsigned* frequencies, - size_t numcodes, unsigned maxbitlen); - -/*Compress a buffer with deflate. See RFC 1951. Out buffer must be freed after use.*/ -unsigned lodepng_deflate(unsigned char** out, size_t* outsize, - const unsigned char* in, size_t insize, - const LodePNGCompressSettings* settings); - -#endif /*LODEPNG_COMPILE_ENCODER*/ -#endif /*LODEPNG_COMPILE_ZLIB*/ - -#ifdef LODEPNG_COMPILE_DISK -/* -Load a file from disk into buffer. The function allocates the out buffer, and -after usage you should free it. -out: output parameter, contains pointer to loaded buffer. -outsize: output parameter, size of the allocated out buffer -filename: the path to the file to load -return value: error code (0 means ok) - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory. -*/ -unsigned lodepng_load_file(unsigned char** out, size_t* outsize, const char* filename); - -/* -Save a file from buffer to disk. Warning, if it exists, this function overwrites -the file without warning! -buffer: the buffer to write -buffersize: size of the buffer to write -filename: the path to the file to save to -return value: error code (0 means ok) - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and encode in-memory -*/ -unsigned lodepng_save_file(const unsigned char* buffer, size_t buffersize, const char* filename); -#endif /*LODEPNG_COMPILE_DISK*/ - -#ifdef LODEPNG_COMPILE_CPP -/* The LodePNG C++ wrapper uses std::vectors instead of manually allocated memory buffers. */ -namespace lodepng { -#ifdef LODEPNG_COMPILE_PNG -class State : public LodePNGState { - public: - State(); - State(const State& other); - ~State(); - State& operator=(const State& other); -}; - -#ifdef LODEPNG_COMPILE_DECODER -/* Same as other lodepng::decode, but using a State for more settings and information. */ -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - State& state, - const unsigned char* in, size_t insize); -unsigned decode(std::vector& out, unsigned& w, unsigned& h, - State& state, - const std::vector& in); -#endif /*LODEPNG_COMPILE_DECODER*/ - -#ifdef LODEPNG_COMPILE_ENCODER -/* Same as other lodepng::encode, but using a State for more settings and information. */ -unsigned encode(std::vector& out, - const unsigned char* in, unsigned w, unsigned h, - State& state); -unsigned encode(std::vector& out, - const std::vector& in, unsigned w, unsigned h, - State& state); -#endif /*LODEPNG_COMPILE_ENCODER*/ - -#ifdef LODEPNG_COMPILE_DISK -/* -Load a file from disk into an std::vector. -return value: error code (0 means ok) - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and decode in-memory -*/ -unsigned load_file(std::vector& buffer, const std::string& filename); - -/* -Save the binary data in an std::vector to a file on disk. The file is overwritten -without warning. - -NOTE: Wide-character filenames are not supported, you can use an external method -to handle such files and encode in-memory -*/ -unsigned save_file(const std::vector& buffer, const std::string& filename); -#endif /* LODEPNG_COMPILE_DISK */ -#endif /* LODEPNG_COMPILE_PNG */ - -#ifdef LODEPNG_COMPILE_ZLIB -#ifdef LODEPNG_COMPILE_DECODER -/* Zlib-decompress an unsigned char buffer */ -unsigned decompress(std::vector& out, const unsigned char* in, size_t insize, - const LodePNGDecompressSettings& settings = lodepng_default_decompress_settings); - -/* Zlib-decompress an std::vector */ -unsigned decompress(std::vector& out, const std::vector& in, - const LodePNGDecompressSettings& settings = lodepng_default_decompress_settings); -#endif /* LODEPNG_COMPILE_DECODER */ - -#ifdef LODEPNG_COMPILE_ENCODER -/* Zlib-compress an unsigned char buffer */ -unsigned compress(std::vector& out, const unsigned char* in, size_t insize, - const LodePNGCompressSettings& settings = lodepng_default_compress_settings); - -/* Zlib-compress an std::vector */ -unsigned compress(std::vector& out, const std::vector& in, - const LodePNGCompressSettings& settings = lodepng_default_compress_settings); -#endif /* LODEPNG_COMPILE_ENCODER */ -#endif /* LODEPNG_COMPILE_ZLIB */ -} /* namespace lodepng */ -#endif /*LODEPNG_COMPILE_CPP*/ - -/* -TODO: -[.] test if there are no memory leaks or security exploits - done a lot but needs to be checked often -[.] check compatibility with various compilers - done but needs to be redone for every newer version -[X] converting color to 16-bit per channel types -[X] support color profile chunk types (but never let them touch RGB values by default) -[ ] support all second edition public PNG chunk types (almost done except sPLT and hIST) -[X] support non-animation third edition public PNG chunk types: eXIf, cICP, mDCV, cLLI -[ ] make sure encoder generates no chunks with size > (2^31)-1 -[ ] partial decoding (stream processing) -[X] let the "isFullyOpaque" function check color keys and transparent palettes too -[X] better name for the variables "codes", "codesD", "codelengthcodes", "clcl" and "lldl" -[ ] allow treating some errors like warnings, when image is recoverable (e.g. 69, 57, 58) -[ ] make warnings like: oob palette, checksum fail, data after iend, wrong/unknown crit chunk, no null terminator in text, ... -[ ] error messages with line numbers (and version) -[ ] errors in state instead of as return code? -[ ] new errors/warnings like suspiciously big decompressed ztxt or iccp chunk -[ ] let the C++ wrapper catch exceptions coming from the standard library and return LodePNG error codes -[ ] allow user to provide custom color conversion functions, e.g. for premultiplied alpha, padding bits or not, ... -[ ] allow user to give data (void*) to custom allocator -[X] provide alternatives for C library functions not present on some platforms (memcpy, ...) -*/ - -#endif /*LODEPNG_H inclusion guard*/ - -/* -LodePNG Documentation ---------------------- - -0. table of contents --------------------- - - 1. about - 1.1. supported features - 1.2. features not supported - 2. C and C++ version - 3. security - 4. decoding - 5. encoding - 6. color conversions - 6.1. PNG color types - 6.2. color conversions - 6.3. padding bits - 6.4. A note about 16-bits per channel and endianness - 7. error values - 8. chunks and PNG editing - 9. compiler support - 10. examples - 10.1. decoder C++ example - 10.2. decoder C example - 11. state settings reference - 12. changes - 13. contact information - - -1. about --------- - -PNG is a file format to store raster images losslessly with good compression, -supporting different color types and alpha channel. - -LodePNG is a PNG codec according to the Portable Network Graphics (PNG) -Specification (Second Edition) - W3C Recommendation 10 November 2003. - -The specifications used are: - -*) Portable Network Graphics (PNG) Specification (Second Edition): - http://www.w3.org/TR/2003/REC-PNG-20031110 -*) RFC 1950 ZLIB Compressed Data Format version 3.3: - http://www.gzip.org/zlib/rfc-zlib.html -*) RFC 1951 DEFLATE Compressed Data Format Specification ver 1.3: - http://www.gzip.org/zlib/rfc-deflate.html - -The most recent version of LodePNG can currently be found at -http://lodev.org/lodepng/ - -LodePNG works both in C (ISO C90) and C++, with a C++ wrapper that adds -extra functionality. - -LodePNG exists out of two files: --lodepng.h: the header file for both C and C++ --lodepng.c(pp): give it the name lodepng.c or lodepng.cpp (or .cc) depending on your usage - -If you want to start using LodePNG right away without reading this doc, get the -examples from the LodePNG website to see how to use it in code, or check the -smaller examples in chapter 13 here. - -LodePNG is simple but only supports the basic requirements. To achieve -simplicity, the following design choices were made: There are no dependencies -on any external library. There are functions to decode and encode a PNG with -a single function call, and extended versions of these functions taking a -LodePNGState struct allowing to specify or get more information. By default -the colors of the raw image are always RGB or RGBA, no matter what color type -the PNG file uses. To read and write files, there are simple functions to -convert the files to/from buffers in memory. - -This all makes LodePNG suitable for loading textures in games, demos and small -programs, ... It's less suitable for full fledged image editors, loading PNGs -over network (it requires all the image data to be available before decoding can -begin), life-critical systems, ... - -1.1. supported features ------------------------ - -The following features are supported by the decoder: - -*) decoding of PNGs with any color type, bit depth and interlace mode, to a 24- or 32-bit color raw image, - or the same color type as the PNG -*) encoding of PNGs, from any raw image to 24- or 32-bit color, or the same color type as the raw image -*) Adam7 interlace and deinterlace for any color type -*) loading the image from harddisk or decoding it from a buffer from other sources than harddisk -*) support for alpha channels, including RGBA color model, translucent palettes and color keying -*) zlib decompression (inflate) -*) zlib compression (deflate) -*) CRC32 and ADLER32 checksums -*) colorimetric color profile conversions: currently experimentally available in lodepng_util.cpp only, - plus alternatively ability to pass on chroma/gamma/ICC profile information to other color management system. -*) handling of unknown chunks, allowing making a PNG editor that stores custom and unknown chunks. -*) the following chunks are supported by both encoder and decoder: - IHDR: header information - PLTE: color palette - IDAT: pixel data - IEND: the final chunk - tRNS: transparency for palettized images - tEXt: textual information - zTXt: compressed textual information - iTXt: international textual information - bKGD: suggested background color - pHYs: physical dimensions - tIME: modification time - cHRM: RGB chromaticities - gAMA: RGB gamma correction - iCCP: ICC color profile - sRGB: rendering intent - sBIT: significant bits - -1.2. features not supported ---------------------------- - -The following features are not (yet) supported: - -*) some features needed to make a conformant PNG-Editor might be still missing. -*) partial loading/stream processing. All data must be available and is processed in one call. -*) The hIST and sPLT public chunks are not (yet) supported but treated as unknown chunks - - -2. C and C++ version --------------------- - -The C version uses buffers allocated with alloc that you need to free() -yourself. You need to use init and cleanup functions for each struct whenever -using a struct from the C version to avoid exploits and memory leaks. - -The C++ version has extra functions with std::vectors in the interface and the -lodepng::State class which is a LodePNGState with constructor and destructor. - -These files work without modification for both C and C++ compilers because all -the additional C++ code is in "#ifdef __cplusplus" blocks that make C-compilers -ignore it, and the C code is made to compile both with strict ISO C90 and C++. - -To use the C++ version, you need to rename the source file to lodepng.cpp -(instead of lodepng.c), and compile it with a C++ compiler. - -To use the C version, you need to rename the source file to lodepng.c (instead -of lodepng.cpp), and compile it with a C compiler. - - -3. Security ------------ - -Even if carefully designed, it's always possible that LodePNG contains possible -exploits. If you discover one, please let me know, and it will be fixed. - -When using LodePNG, care has to be taken with the C version of LodePNG, as well -as the C-style structs when working with C++. The following conventions are used -for all C-style structs: - --if a struct has a corresponding init function, always call the init function when making a new one --if a struct has a corresponding cleanup function, call it before the struct disappears to avoid memory leaks --if a struct has a corresponding copy function, use the copy function instead of "=". - The destination must also be inited already. - - -4. Decoding ------------ - -Decoding converts a PNG compressed image to a raw pixel buffer. - -Most documentation on using the decoder is at its declarations in the header -above. For C, simple decoding can be done with functions such as -lodepng_decode32, and more advanced decoding can be done with the struct -LodePNGState and lodepng_decode. For C++, all decoding can be done with the -various lodepng::decode functions, and lodepng::State can be used for advanced -features. - -When using the LodePNGState, it uses the following fields for decoding: -*) LodePNGInfo info_png: it stores extra information about the PNG (the input) in here -*) LodePNGColorMode info_raw: here you can say what color mode of the raw image (the output) you want to get -*) LodePNGDecoderSettings decoder: you can specify a few extra settings for the decoder to use - -LodePNGInfo info_png --------------------- - -After decoding, this contains extra information of the PNG image, except the actual -pixels, width and height because these are already gotten directly from the decoder -functions. - -It contains for example the original color type of the PNG image, text comments, -suggested background color, etc... More details about the LodePNGInfo struct are -at its declaration documentation. - -LodePNGColorMode info_raw -------------------------- - -When decoding, here you can specify which color type you want -the resulting raw image to be. If this is different from the colortype of the -PNG, then the decoder will automatically convert the result. This conversion -always works, except if you want it to convert a color PNG to grayscale or to -a palette with missing colors. - -By default, 32-bit color is used for the result. - -LodePNGDecoderSettings decoder ------------------------------- - -The settings can be used to ignore the errors created by invalid CRC and Adler32 -chunks, and to disable the decoding of tEXt chunks. - -There's also a setting color_convert, true by default. If false, no conversion -is done, the resulting data will be as it was in the PNG (after decompression) -and you'll have to puzzle the colors of the pixels together yourself using the -color type information in the LodePNGInfo. - - -5. Encoding ------------ - -Encoding converts a raw pixel buffer to a PNG compressed image. - -Most documentation on using the encoder is at its declarations in the header -above. For C, simple encoding can be done with functions such as -lodepng_encode32, and more advanced decoding can be done with the struct -LodePNGState and lodepng_encode. For C++, all encoding can be done with the -various lodepng::encode functions, and lodepng::State can be used for advanced -features. - -Like the decoder, the encoder can also give errors. However it gives less errors -since the encoder input is trusted, the decoder input (a PNG image that could -be forged by anyone) is not trusted. - -When using the LodePNGState, it uses the following fields for encoding: -*) LodePNGInfo info_png: here you specify how you want the PNG (the output) to be. -*) LodePNGColorMode info_raw: here you say what color type of the raw image (the input) has -*) LodePNGEncoderSettings encoder: you can specify a few settings for the encoder to use - -LodePNGInfo info_png --------------------- - -When encoding, you use this the opposite way as when decoding: for encoding, -you fill in the values you want the PNG to have before encoding. By default it's -not needed to specify a color type for the PNG since it's automatically chosen, -but it's possible to choose it yourself given the right settings. - -The encoder will not always exactly match the LodePNGInfo struct you give, -it tries as close as possible. Some things are ignored by the encoder. The -encoder uses, for example, the following settings from it when applicable: -colortype and bitdepth, text chunks, time chunk, the color key, the palette, the -background color, the interlace method, unknown chunks, ... - -When encoding to a PNG with colortype 3, the encoder will generate a PLTE chunk. -If the palette contains any colors for which the alpha channel is not 255 (so -there are translucent colors in the palette), it'll add a tRNS chunk. - -LodePNGColorMode info_raw -------------------------- - -You specify the color type of the raw image that you give to the input here, -including a possible transparent color key and palette you happen to be using in -your raw image data. - -By default, 32-bit color is assumed, meaning your input has to be in RGBA -format with 4 bytes (unsigned chars) per pixel. - -LodePNGEncoderSettings encoder ------------------------------- - -The following settings are supported (some are in sub-structs): -*) auto_convert: when this option is enabled, the encoder will -automatically choose the smallest possible color mode (including color key) that -can encode the colors of all pixels without information loss. -*) btype: the block type for LZ77. 0 = uncompressed, 1 = fixed huffman tree, - 2 = dynamic huffman tree (best compression). Should be 2 for proper - compression. -*) use_lz77: whether or not to use LZ77 for compressed block types. Should be - true for proper compression. -*) windowsize: the window size used by the LZ77 encoder (1 - 32768). Has value - 2048 by default, but can be set to 32768 for better, but slow, compression. -*) force_palette: if colortype is 2 or 6, you can make the encoder write a PLTE - chunk if force_palette is true. This can used as suggested palette to convert - to by viewers that don't support more than 256 colors (if those still exist) -*) add_id: add text chunk "Encoder: LodePNG " to the image. -*) text_compression: default 1. If 1, it'll store texts as zTXt instead of tEXt chunks. - zTXt chunks use zlib compression on the text. This gives a smaller result on - large texts but a larger result on small texts (such as a single program name). - It's all tEXt or all zTXt though, there's no separate setting per text yet. - - -6. color conversions --------------------- - -An important thing to note about LodePNG, is that the color type of the PNG, and -the color type of the raw image, are completely independent. By default, when -you decode a PNG, you get the result as a raw image in the color type you want, -no matter whether the PNG was encoded with a palette, grayscale or RGBA color. -And if you encode an image, by default LodePNG will automatically choose the PNG -color type that gives good compression based on the values of colors and amount -of colors in the image. It can be configured to let you control it instead as -well, though. - -To be able to do this, LodePNG does conversions from one color mode to another. -It can convert from almost any color type to any other color type, except the -following conversions: RGB to grayscale is not supported, and converting to a -palette when the palette doesn't have a required color is not supported. This is -not supported on purpose: this is information loss which requires a color -reduction algorithm that is beyond the scope of a PNG encoder (yes, RGB to gray -is easy, but there are multiple ways if you want to give some channels more -weight). - -By default, when decoding, you get the raw image in 32-bit RGBA or 24-bit RGB -color, no matter what color type the PNG has. And by default when encoding, -LodePNG automatically picks the best color model for the output PNG, and expects -the input image to be 32-bit RGBA or 24-bit RGB. So, unless you want to control -the color format of the images yourself, you can skip this chapter. - -6.1. PNG color types --------------------- - -A PNG image can have many color types, ranging from 1-bit color to 64-bit color, -as well as palettized color modes. After the zlib decompression and unfiltering -in the PNG image is done, the raw pixel data will have that color type and thus -a certain amount of bits per pixel. If you want the output raw image after -decoding to have another color type, a conversion is done by LodePNG. - -The PNG specification gives the following color types: - -0: grayscale, bit depths 1, 2, 4, 8, 16 -2: RGB, bit depths 8 and 16 -3: palette, bit depths 1, 2, 4 and 8 -4: grayscale with alpha, bit depths 8 and 16 -6: RGBA, bit depths 8 and 16 - -Bit depth is the amount of bits per pixel per color channel. So the total amount -of bits per pixel is: amount of channels * bitdepth. - -6.2. color conversions ----------------------- - -As explained in the sections about the encoder and decoder, you can specify -color types and bit depths in info_png and info_raw to change the default -behaviour. - -If, when decoding, you want the raw image to be something else than the default, -you need to set the color type and bit depth you want in the LodePNGColorMode, -or the parameters colortype and bitdepth of the simple decoding function. - -If, when encoding, you use another color type than the default in the raw input -image, you need to specify its color type and bit depth in the LodePNGColorMode -of the raw image, or use the parameters colortype and bitdepth of the simple -encoding function. - -If, when encoding, you don't want LodePNG to choose the output PNG color type -but control it yourself, you need to set auto_convert in the encoder settings -to false, and specify the color type you want in the LodePNGInfo of the -encoder (including palette: it can generate a palette if auto_convert is true, -otherwise not). - -If the input and output color type differ (whether user chosen or auto chosen), -LodePNG will do a color conversion, which follows the rules below, and may -sometimes result in an error. - -To avoid some confusion: --the decoder converts from PNG to raw image --the encoder converts from raw image to PNG --the colortype and bitdepth in LodePNGColorMode info_raw, are those of the raw image --the colortype and bitdepth in the color field of LodePNGInfo info_png, are those of the PNG --when encoding, the color type in LodePNGInfo is ignored if auto_convert - is enabled, it is automatically generated instead --when decoding, the color type in LodePNGInfo is set by the decoder to that of the original - PNG image, but it can be ignored since the raw image has the color type you requested instead --if the color type of the LodePNGColorMode and PNG image aren't the same, a conversion - between the color types is done if the color types are supported. If it is not - supported, an error is returned. If the types are the same, no conversion is done. --even though some conversions aren't supported, LodePNG supports loading PNGs from any - colortype and saving PNGs to any colortype, sometimes it just requires preparing - the raw image correctly before encoding. --both encoder and decoder use the same color converter. - -The function lodepng_convert does the color conversion. It is available in the -interface but normally isn't needed since the encoder and decoder already call -it. - -Non supported color conversions: --color to grayscale when non-gray pixels are present: no error is thrown, but -the result will look ugly because only the red channel is taken (it assumes all -three channels are the same in this case so ignores green and blue). The reason -no error is given is to allow converting from three-channel grayscale images to -one-channel even if there are numerical imprecisions. --anything to palette when the palette does not have an exact match for a from-color -in it: in this case an error is thrown - -Supported color conversions: --anything to 8-bit RGB, 8-bit RGBA, 16-bit RGB, 16-bit RGBA --any gray or gray+alpha, to gray or gray+alpha --anything to a palette, as long as the palette has the requested colors in it --removing alpha channel --higher to smaller bitdepth, and vice versa - -If you want no color conversion to be done (e.g. for speed or control): --In the encoder, you can make it save a PNG with any color type by giving the -raw color mode and LodePNGInfo the same color mode, and setting auto_convert to -false. --In the decoder, you can make it store the pixel data in the same color type -as the PNG has, by setting the color_convert setting to false. Settings in -info_raw are then ignored. - -6.3. padding bits ------------------ - -In the PNG file format, if a less than 8-bit per pixel color type is used and the scanlines -have a bit amount that isn't a multiple of 8, then padding bits are used so that each -scanline starts at a fresh byte. But that is NOT true for the LodePNG raw input and output. -The raw input image you give to the encoder, and the raw output image you get from the decoder -will NOT have these padding bits, e.g. in the case of a 1-bit image with a width -of 7 pixels, the first pixel of the second scanline will the 8th bit of the first byte, -not the first bit of a new byte. - -6.4. A note about 16-bits per channel and endianness ----------------------------------------------------- - -LodePNG uses unsigned char arrays for 16-bit per channel colors too, just like -for any other color format. The 16-bit values are stored in big endian (most -significant byte first) in these arrays. This is the opposite order of the -little endian used by x86 CPU's. - -LodePNG always uses big endian because the PNG file format does so internally. -Conversions to other formats than PNG uses internally are not supported by -LodePNG on purpose, there are myriads of formats, including endianness of 16-bit -colors, the order in which you store R, G, B and A, and so on. Supporting and -converting to/from all that is outside the scope of LodePNG. - -This may mean that, depending on your use case, you may want to convert the big -endian output of LodePNG to little endian with a for loop. This is certainly not -always needed, many applications and libraries support big endian 16-bit colors -anyway, but it means you cannot simply cast the unsigned char* buffer to an -unsigned short* buffer on x86 CPUs. - - -7. error values ---------------- - -All functions in LodePNG that return an error code, return 0 if everything went -OK, or a non-zero code if there was an error. - -The meaning of the LodePNG error values can be retrieved with the function -lodepng_error_text: given the numerical error code, it returns a description -of the error in English as a string. - -Check the implementation of lodepng_error_text to see the meaning of each code. - -It is not recommended to use the numerical values to programmatically make -different decisions based on error types as the numbers are not guaranteed to -stay backwards compatible. They are for human consumption only. Programmatically -only 0 or non-0 matter. - - -8. chunks and PNG editing -------------------------- - -If you want to add extra chunks to a PNG you encode, or use LodePNG for a PNG -editor that should follow the rules about handling of unknown chunks, or if your -program is able to read other types of chunks than the ones handled by LodePNG, -then that's possible with the chunk functions of LodePNG. - -A PNG chunk has the following layout: - -4 bytes length -4 bytes type name -length bytes data -4 bytes CRC - -8.1. iterating through chunks ------------------------------ - -If you have a buffer containing the PNG image data, then the first chunk (the -IHDR chunk) starts at byte number 8 of that buffer. The first 8 bytes are the -signature of the PNG and are not part of a chunk. But if you start at byte 8 -then you have a chunk, and can check the following things of it. - -NOTE: none of these functions check for memory buffer boundaries. To avoid -exploits, always make sure the buffer contains all the data of the chunks. -When using lodepng_chunk_next, make sure the returned value is within the -allocated memory. - -unsigned lodepng_chunk_length(const unsigned char* chunk): - -Get the length of the chunk's data. The total chunk length is this length + 12. - -void lodepng_chunk_type(char type[5], const unsigned char* chunk): -unsigned char lodepng_chunk_type_equals(const unsigned char* chunk, const char* type): - -Get the type of the chunk or compare if it's a certain type - -unsigned char lodepng_chunk_critical(const unsigned char* chunk): -unsigned char lodepng_chunk_private(const unsigned char* chunk): -unsigned char lodepng_chunk_safetocopy(const unsigned char* chunk): - -Check if the chunk is critical in the PNG standard (only IHDR, PLTE, IDAT and IEND are). -Check if the chunk is private (public chunks are part of the standard, private ones not). -Check if the chunk is safe to copy. If it's not, then, when modifying data in a critical -chunk, unsafe to copy chunks of the old image may NOT be saved in the new one if your -program doesn't handle that type of unknown chunk. - -unsigned char* lodepng_chunk_data(unsigned char* chunk): -const unsigned char* lodepng_chunk_data_const(const unsigned char* chunk): - -Get a pointer to the start of the data of the chunk. - -unsigned lodepng_chunk_check_crc(const unsigned char* chunk): -void lodepng_chunk_generate_crc(unsigned char* chunk): - -Check if the crc is correct or generate a correct one. - -unsigned char* lodepng_chunk_next(unsigned char* chunk): -const unsigned char* lodepng_chunk_next_const(const unsigned char* chunk): - -Iterate to the next chunk. This works if you have a buffer with consecutive chunks. Note that these -functions do no boundary checking of the allocated data whatsoever, so make sure there is enough -data available in the buffer to be able to go to the next chunk. - -unsigned lodepng_chunk_append(unsigned char** out, size_t* outsize, const unsigned char* chunk): -unsigned lodepng_chunk_create(unsigned char** out, size_t* outsize, unsigned length, - const char* type, const unsigned char* data): - -These functions are used to create new chunks that are appended to the data in *out that has -length *outsize. The append function appends an existing chunk to the new data. The create -function creates a new chunk with the given parameters and appends it. Type is the 4-letter -name of the chunk. - -8.2. chunks in info_png ------------------------ - -The LodePNGInfo struct contains fields with the unknown chunk in it. It has 3 -buffers (each with size) to contain 3 types of unknown chunks: -the ones that come before the PLTE chunk, the ones that come between the PLTE -and the IDAT chunks, and the ones that come after the IDAT chunks. -It's necessary to make the distinction between these 3 cases because the PNG -standard forces to keep the ordering of unknown chunks compared to the critical -chunks, but does not force any other ordering rules. - -info_png.unknown_chunks_data[0] is the chunks before PLTE -info_png.unknown_chunks_data[1] is the chunks after PLTE, before IDAT -info_png.unknown_chunks_data[2] is the chunks after IDAT - -The chunks in these 3 buffers can be iterated through and read by using the same -way described in the previous subchapter. - -When using the decoder to decode a PNG, you can make it store all unknown chunks -if you set the option settings.remember_unknown_chunks to 1. By default, this -option is off (0). - -The encoder will always encode unknown chunks that are stored in the info_png. -If you need it to add a particular chunk that isn't known by LodePNG, you can -use lodepng_chunk_append or lodepng_chunk_create to the chunk data in -info_png.unknown_chunks_data[x]. - -Chunks that are known by LodePNG should not be added in that way. E.g. to make -LodePNG add a bKGD chunk, set background_defined to true and add the correct -parameters there instead. - - -9. compiler support -------------------- - -No libraries other than the current standard C library are needed to compile -LodePNG. For the C++ version, only the standard C++ library is needed on top. -Add the files lodepng.c(pp) and lodepng.h to your project, include -lodepng.h where needed, and your program can read/write PNG files. - -It is compatible with C90 and up, and C++03 and up. - -If performance is important, use optimization when compiling! For both the -encoder and decoder, this makes a large difference. - -Make sure that LodePNG is compiled with the same compiler of the same version -and with the same settings as the rest of the program, or the interfaces with -std::vectors and std::strings in C++ can be incompatible. - -CHAR_BITS must be 8 or higher, because LodePNG uses unsigned chars for octets. - -*) gcc and g++ - -LodePNG is developed in gcc so this compiler is natively supported. It gives no -warnings with compiler options "-Wall -Wextra -pedantic -ansi", with gcc and g++ -version 4.7.1 on Linux, 32-bit and 64-bit. - -*) Clang - -Fully supported and warning-free. - -*) Mingw - -The Mingw compiler (a port of gcc for Windows) should be fully supported by -LodePNG. - -*) Visual Studio and Visual C++ Express Edition - -LodePNG should be warning-free with warning level W4. Two warnings were disabled -with pragmas though: warning 4244 about implicit conversions, and warning 4996 -where it wants to use a non-standard function fopen_s instead of the standard C -fopen. - -Visual Studio may want "stdafx.h" files to be included in each source file and -give an error "unexpected end of file while looking for precompiled header". -This is not standard C++ and will not be added to the stock LodePNG. You can -disable it for lodepng.cpp only by right clicking it, Properties, C/C++, -Precompiled Headers, and set it to Not Using Precompiled Headers there. - -NOTE: Modern versions of VS should be fully supported, but old versions, e.g. -VS6, are not guaranteed to work. - -*) Compilers on Macintosh - -LodePNG has been reported to work both with gcc and LLVM for Macintosh, both for -C and C++. - -*) Other Compilers - -If you encounter problems on any compilers, feel free to let me know and I may -try to fix it if the compiler is modern and standards compliant. - - -10. examples ------------- - -This decoder example shows the most basic usage of LodePNG. More complex -examples can be found on the LodePNG website. - -NOTE: these examples do not support wide-character filenames, you can use an -external method to handle such files and encode or decode in-memory - -10.1. decoder C++ example -------------------------- - -#include "lodepng.h" -#include - -int main(int argc, char *argv[]) { - const char* filename = argc > 1 ? argv[1] : "test.png"; - - //load and decode - std::vector image; - unsigned width, height; - unsigned error = lodepng::decode(image, width, height, filename); - - //if there's an error, display it - if(error) std::cout << "decoder error " << error << ": " << lodepng_error_text(error) << std::endl; - - //the pixels are now in the vector "image", 4 bytes per pixel, ordered RGBARGBA..., use it as texture, draw it, ... -} - -10.2. decoder C example ------------------------ - -#include "lodepng.h" - -int main(int argc, char *argv[]) { - unsigned error; - unsigned char* image; - size_t width, height; - const char* filename = argc > 1 ? argv[1] : "test.png"; - - error = lodepng_decode32_file(&image, &width, &height, filename); - - if(error) printf("decoder error %u: %s\n", error, lodepng_error_text(error)); - - / * use image here * / - - free(image); - return 0; -} - -11. state settings reference ----------------------------- - -A quick reference of some settings to set on the LodePNGState - -For decoding: - -state.decoder.zlibsettings.ignore_adler32: ignore ADLER32 checksums -state.decoder.zlibsettings.custom_...: use custom inflate function -state.decoder.ignore_crc: ignore CRC checksums -state.decoder.ignore_critical: ignore unknown critical chunks -state.decoder.ignore_end: ignore missing IEND chunk. May fail if this corruption causes other errors -state.decoder.color_convert: convert internal PNG color to chosen one -state.decoder.read_text_chunks: whether to read in text metadata chunks -state.decoder.remember_unknown_chunks: whether to read in unknown chunks -state.info_raw.colortype: desired color type for decoded image -state.info_raw.bitdepth: desired bit depth for decoded image -state.info_raw....: more color settings, see struct LodePNGColorMode -state.info_png....: no settings for decoder but ouput, see struct LodePNGInfo - -For encoding: - -state.encoder.zlibsettings.btype: disable compression by setting it to 0 -state.encoder.zlibsettings.use_lz77: use LZ77 in compression -state.encoder.zlibsettings.windowsize: tweak LZ77 windowsize -state.encoder.zlibsettings.minmatch: tweak min LZ77 length to match -state.encoder.zlibsettings.nicematch: tweak LZ77 match where to stop searching -state.encoder.zlibsettings.lazymatching: try one more LZ77 matching -state.encoder.zlibsettings.custom_...: use custom deflate function -state.encoder.auto_convert: choose optimal PNG color type, if 0 uses info_png -state.encoder.filter_palette_zero: PNG filter strategy for palette -state.encoder.filter_strategy: PNG filter strategy to encode with -state.encoder.force_palette: add palette even if not encoding to one -state.encoder.add_id: add LodePNG identifier and version as a text chunk -state.encoder.text_compression: use compressed text chunks for metadata -state.info_raw.colortype: color type of raw input image you provide -state.info_raw.bitdepth: bit depth of raw input image you provide -state.info_raw: more color settings, see struct LodePNGColorMode -state.info_png.color.colortype: desired color type if auto_convert is false -state.info_png.color.bitdepth: desired bit depth if auto_convert is false -state.info_png.color....: more color settings, see struct LodePNGColorMode -state.info_png....: more PNG related settings, see struct LodePNGInfo - - -12. changes ------------ - -The version number of LodePNG is the date of the change given in the format -yyyymmdd. - -Some changes aren't backwards compatible. Those are indicated with a (!) -symbol. - -Not all changes are listed here, the commit history in github lists more: -https://github.com/lvandeve/lodepng - -*) 6 may 2025: renamed mDCv to mDCV and cLLi to cLLI as per the recent rename - in the draft png third edition spec. Please note that while the third - edition is not finalized, backwards-incompatible changes to its features are - possible. -*) 23 dec 2024: added support for the mDCv and cLLi chunks (for png third - edition spec) -*) 22 dec 2024: added support for the cICP chunk (for png third edition spec) -*) 15 dec 2024: added support for the eXIf chunk (for png third edition spec) -*) 10 apr 2023: faster CRC32 implementation, but with larger lookup table. -*) 13 jun 2022: added support for the sBIT chunk. -*) 09 jan 2022: minor decoder speed improvements. -*) 27 jun 2021: added warnings that file reading/writing functions don't support - wide-character filenames (support for this is not planned, opening files is - not the core part of PNG decoding/decoding and is platform dependent). -*) 17 oct 2020: prevent decoding too large text/icc chunks by default. -*) 06 mar 2020: simplified some of the dynamic memory allocations. -*) 12 jan 2020: (!) added 'end' argument to lodepng_chunk_next to allow correct - overflow checks. -*) 14 aug 2019: around 25% faster decoding thanks to huffman lookup tables. -*) 15 jun 2019: (!) auto_choose_color API changed (for bugfix: don't use palette - if gray ICC profile) and non-ICC LodePNGColorProfile renamed to - LodePNGColorStats. -*) 30 dec 2018: code style changes only: removed newlines before opening braces. -*) 10 sep 2018: added way to inspect metadata chunks without full decoding. -*) 19 aug 2018: (!) fixed color mode bKGD is encoded with and made it use - palette index in case of palette. -*) 10 aug 2018: (!) added support for gAMA, cHRM, sRGB and iCCP chunks. This - change is backwards compatible unless you relied on unknown_chunks for those. -*) 11 jun 2018: less restrictive check for pixel size integer overflow -*) 14 jan 2018: allow optionally ignoring a few more recoverable errors -*) 17 sep 2017: fix memory leak for some encoder input error cases -*) 27 nov 2016: grey+alpha auto color model detection bugfix -*) 18 apr 2016: Changed qsort to custom stable sort (for platforms w/o qsort). -*) 09 apr 2016: Fixed colorkey usage detection, and better file loading (within - the limits of pure C90). -*) 08 dec 2015: Made load_file function return error if file can't be opened. -*) 24 oct 2015: Bugfix with decoding to palette output. -*) 18 apr 2015: Boundary PM instead of just package-merge for faster encoding. -*) 24 aug 2014: Moved to github -*) 23 aug 2014: Reduced needless memory usage of decoder. -*) 28 jun 2014: Removed fix_png setting, always support palette OOB for - simplicity. Made ColorProfile public. -*) 09 jun 2014: Faster encoder by fixing hash bug and more zeros optimization. -*) 22 dec 2013: Power of two windowsize required for optimization. -*) 15 apr 2013: Fixed bug with LAC_ALPHA and color key. -*) 25 mar 2013: Added an optional feature to ignore some PNG errors (fix_png). -*) 11 mar 2013: (!) Bugfix with custom free. Changed from "my" to "lodepng_" - prefix for the custom allocators and made it possible with a new #define to - use custom ones in your project without needing to change lodepng's code. -*) 28 jan 2013: Bugfix with color key. -*) 27 oct 2012: Tweaks in text chunk keyword length error handling. -*) 8 oct 2012: (!) Added new filter strategy (entropy) and new auto color mode. - (no palette). Better deflate tree encoding. New compression tweak settings. - Faster color conversions while decoding. Some internal cleanups. -*) 23 sep 2012: Reduced warnings in Visual Studio a little bit. -*) 1 sep 2012: (!) Removed #define's for giving custom (de)compression functions - and made it work with function pointers instead. -*) 23 jun 2012: Added more filter strategies. Made it easier to use custom alloc - and free functions and toggle #defines from compiler flags. Small fixes. -*) 6 may 2012: (!) Made plugging in custom zlib/deflate functions more flexible. -*) 22 apr 2012: (!) Made interface more consistent, renaming a lot. Removed - redundant C++ codec classes. Reduced amount of structs. Everything changed, - but it is cleaner now imho and functionality remains the same. Also fixed - several bugs and shrunk the implementation code. Made new samples. -*) 6 nov 2011: (!) By default, the encoder now automatically chooses the best - PNG color model and bit depth, based on the amount and type of colors of the - raw image. For this, autoLeaveOutAlphaChannel replaced by auto_choose_color. -*) 9 oct 2011: simpler hash chain implementation for the encoder. -*) 8 sep 2011: lz77 encoder lazy matching instead of greedy matching. -*) 23 aug 2011: tweaked the zlib compression parameters after benchmarking. - A bug with the PNG filtertype heuristic was fixed, so that it chooses much - better ones (it's quite significant). A setting to do an experimental, slow, - brute force search for PNG filter types is added. -*) 17 aug 2011: (!) changed some C zlib related function names. -*) 16 aug 2011: made the code less wide (max 120 characters per line). -*) 17 apr 2011: code cleanup. Bugfixes. Convert low to 16-bit per sample colors. -*) 21 feb 2011: fixed compiling for C90. Fixed compiling with sections disabled. -*) 11 dec 2010: encoding is made faster, based on suggestion by Peter Eastman - to optimize long sequences of zeros. -*) 13 nov 2010: added LodePNG_InfoColor_hasPaletteAlpha and - LodePNG_InfoColor_canHaveAlpha functions for convenience. -*) 7 nov 2010: added LodePNG_error_text function to get error code description. -*) 30 oct 2010: made decoding slightly faster -*) 26 oct 2010: (!) changed some C function and struct names (more consistent). - Reorganized the documentation and the declaration order in the header. -*) 08 aug 2010: only changed some comments and external samples. -*) 05 jul 2010: fixed bug thanks to warnings in the new gcc version. -*) 14 mar 2010: fixed bug where too much memory was allocated for char buffers. -*) 02 sep 2008: fixed bug where it could create empty tree that linux apps could - read by ignoring the problem but windows apps couldn't. -*) 06 jun 2008: added more error checks for out of memory cases. -*) 26 apr 2008: added a few more checks here and there to ensure more safety. -*) 06 mar 2008: crash with encoding of strings fixed -*) 02 feb 2008: support for international text chunks added (iTXt) -*) 23 jan 2008: small cleanups, and #defines to divide code in sections -*) 20 jan 2008: support for unknown chunks allowing using LodePNG for an editor. -*) 18 jan 2008: support for tIME and pHYs chunks added to encoder and decoder. -*) 17 jan 2008: ability to encode and decode compressed zTXt chunks added - Also various fixes, such as in the deflate and the padding bits code. -*) 13 jan 2008: Added ability to encode Adam7-interlaced images. Improved - filtering code of encoder. -*) 07 jan 2008: (!) changed LodePNG to use ISO C90 instead of C++. A - C++ wrapper around this provides an interface almost identical to before. - Having LodePNG be pure ISO C90 makes it more portable. The C and C++ code - are together in these files but it works both for C and C++ compilers. -*) 29 dec 2007: (!) changed most integer types to unsigned int + other tweaks -*) 30 aug 2007: bug fixed which makes this Borland C++ compatible -*) 09 aug 2007: some VS2005 warnings removed again -*) 21 jul 2007: deflate code placed in new namespace separate from zlib code -*) 08 jun 2007: fixed bug with 2- and 4-bit color, and small interlaced images -*) 04 jun 2007: improved support for Visual Studio 2005: crash with accessing - invalid std::vector element [0] fixed, and level 3 and 4 warnings removed -*) 02 jun 2007: made the encoder add a tag with version by default -*) 27 may 2007: zlib and png code separated (but still in the same file), - simple encoder/decoder functions added for more simple usage cases -*) 19 may 2007: minor fixes, some code cleaning, new error added (error 69), - moved some examples from here to lodepng_examples.cpp -*) 12 may 2007: palette decoding bug fixed -*) 24 apr 2007: changed the license from BSD to the zlib license -*) 11 mar 2007: very simple addition: ability to encode bKGD chunks. -*) 04 mar 2007: (!) tEXt chunk related fixes, and support for encoding - palettized PNG images. Plus little interface change with palette and texts. -*) 03 mar 2007: Made it encode dynamic Huffman shorter with repeat codes. - Fixed a bug where the end code of a block had length 0 in the Huffman tree. -*) 26 feb 2007: Huffman compression with dynamic trees (BTYPE 2) now implemented - and supported by the encoder, resulting in smaller PNGs at the output. -*) 27 jan 2007: Made the Adler-32 test faster so that a timewaste is gone. -*) 24 jan 2007: gave encoder an error interface. Added color conversion from any - greyscale type to 8-bit greyscale with or without alpha. -*) 21 jan 2007: (!) Totally changed the interface. It allows more color types - to convert to and is more uniform. See the manual for how it works now. -*) 07 jan 2007: Some cleanup & fixes, and a few changes over the last days: - encode/decode custom tEXt chunks, separate classes for zlib & deflate, and - at last made the decoder give errors for incorrect Adler32 or Crc. -*) 01 jan 2007: Fixed bug with encoding PNGs with less than 8 bits per channel. -*) 29 dec 2006: Added support for encoding images without alpha channel, and - cleaned out code as well as making certain parts faster. -*) 28 dec 2006: Added "Settings" to the encoder. -*) 26 dec 2006: The encoder now does LZ77 encoding and produces much smaller files now. - Removed some code duplication in the decoder. Fixed little bug in an example. -*) 09 dec 2006: (!) Placed output parameters of public functions as first parameter. - Fixed a bug of the decoder with 16-bit per color. -*) 15 oct 2006: Changed documentation structure -*) 09 oct 2006: Encoder class added. It encodes a valid PNG image from the - given image buffer, however for now it's not compressed. -*) 08 sep 2006: (!) Changed to interface with a Decoder class -*) 30 jul 2006: (!) LodePNG_InfoPng , width and height are now retrieved in different - way. Renamed decodePNG to decodePNGGeneric. -*) 29 jul 2006: (!) Changed the interface: image info is now returned as a - struct of type LodePNG::LodePNG_Info, instead of a vector, which was a bit clumsy. -*) 28 jul 2006: Cleaned the code and added new error checks. - Corrected terminology "deflate" into "inflate". -*) 23 jun 2006: Added SDL example in the documentation in the header, this - example allows easy debugging by displaying the PNG and its transparency. -*) 22 jun 2006: (!) Changed way to obtain error value. Added - loadFile function for convenience. Made decodePNG32 faster. -*) 21 jun 2006: (!) Changed type of info vector to unsigned. - Changed position of palette in info vector. Fixed an important bug that - happened on PNGs with an uncompressed block. -*) 16 jun 2006: Internally changed unsigned into unsigned where - needed, and performed some optimizations. -*) 07 jun 2006: (!) Renamed functions to decodePNG and placed them - in LodePNG namespace. Changed the order of the parameters. Rewrote the - documentation in the header. Renamed files to lodepng.cpp and lodepng.h -*) 22 apr 2006: Optimized and improved some code -*) 07 sep 2005: (!) Changed to std::vector interface -*) 12 aug 2005: Initial release (C++, decoder only) -*/ diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h index e505ecb8..9af82f7a 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h @@ -10,16 +10,19 @@ #include #include +#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 "common/base/constants.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" +#include namespace cmvr { class PiPGlfwAdapter; class MuJocoViewer { friend class PiPGlfwAdapter; public: - explicit MuJocoViewer(const char *model_path); + explicit MuJocoViewer(std::shared_ptr world); ~MuJocoViewer(); @@ -32,8 +35,8 @@ namespace cmvr { void setRunning(bool running); void requestStop(); - mjModel *model() const { return m_; } - mjData *data() const { return d_; } + mjModel *model() const; + 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: - // 每次 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); - } - + public: 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 physicsThreadFunc(); // 加载模型 + 物理循环 - void physicsLoop(); // 真正的一步一步仿真 - void controlDispatch(mjModel *m, mjData *d); - - - static mjModel *LoadModelSimple(const char *file, - char *load_error, - int error_sz); + void initSim(); + void syncThreadFunc(); private: - std::string model_path_; - - mjModel *m_ = nullptr; - mjData *d_ = nullptr; + std::shared_ptr world_; mjvCamera cam_; mjvOption opt_; mjvPerturb pert_; std::unique_ptr sim_; - std::thread physics_thread_; - std::atomic physics_started_{false}; - - - bool inited_ = false; - mjtNum last_time_ = 0.0; + std::thread sync_thread_; 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& rgb, + std::vector& depth, + int& width, + int& height, + uint64_t& frame_id) const; + + private: + config::MujocoViewerConfig config_; + config::MujocoCameraConfig pip_camera_config_; + std::shared_ptr world_; + std::unique_ptr viewer_; + std::thread viewer_thread_; + mutable std::mutex mtx_; + bool has_pip_camera_config_ = false; + bool running_ = false; + bool stop_requested_ = false; + }; } // namespace cmvr diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h deleted file mode 100644 index 64b0211d..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h +++ /dev/null @@ -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 - -#include - -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 GetCursorPosition() const = 0; - virtual double GetDisplayPixelsPerInch() const = 0; - virtual std::pair GetFramebufferSize() const = 0; - virtual std::pair 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_ diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/simulate.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/simulate.h deleted file mode 100644 index 567a1226..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/simulate.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#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; - -// Simulate states not contained in MuJoCo structures -class Simulate { - public: - using Clock = std::chrono::steady_clock; - static_assert(std::ratio_less_equal_v); - - static constexpr int kMaxGeom = 100000; - - // create object and initialize the simulate ui - Simulate( - std::unique_ptr 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 body_parentid_; - - std::vector jnt_type_; - std::vector jnt_group_; - std::vector jnt_qposadr_; - std::vector>> jnt_range_; - std::vector jnt_names_; - - std::vector actuator_group_; - std::vector>> actuator_ctrlrange_; - std::vector actuator_names_; - - std::vector equality_names_; - - std::vector history_; // history buffer (nhistory x state_size) - - // mjModel and mjData fields that can be modified by the user through the GUI - std::vector qpos_; - std::vector qpos_prev_; - std::vector ctrl_; - std::vector ctrl_prev_; - std::vector eq_active_; - std::vector 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 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 save_xml; - std::optional save_mjb; - std::optional print_model; - std::optional 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 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> user_figures_; - std::vector> user_figures_new_; - std::vector> user_texts_; - std::vector> user_texts_new_; - std::vector>> user_images_; - std::vector>> user_images_new_; - - // OpenGL rendering and UI - int refresh_rate = 60; - int window_pos[2] = {0}; - int window_size[2] = {0}; - std::unique_ptr 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 diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_adapter.cc b/cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_adapter.cc deleted file mode 100644 index e38ab5e2..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_adapter.cc +++ /dev/null @@ -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 -#include - -#include -#include -#include -#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(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 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); -#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 diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_dispatch.cc b/cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_dispatch.cc deleted file mode 100644 index 9c0effec..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/glfw_dispatch.cc +++ /dev/null @@ -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 - #include - #else - #include - #endif -#endif - -#include -#include - -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( \ - 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 -#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 diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp index ed088aa1..300456dd 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp @@ -4,22 +4,20 @@ #include -#include #include #include +#include #include +#include +#include + +#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/mujoco/mujoco_viewer/include/glfw_adapter.h" +#include namespace cmvr { namespace mj = ::mujoco; - namespace mju = ::mujoco::sample_util; - using Seconds = std::chrono::duration; - - 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_; }; - - static const char *CheckDiverged(int disableflags, const mjData *d) { - 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 : "") { + MuJocoViewer::MuJocoViewer(std::shared_ptr world) + : world_(std::move(world)) { 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( 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()) { - physics_thread_.join(); + if (sync_thread_.joinable()) { + 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() { - // 1) 加载模型 - char error[1024]; - m_ = LoadModelSimple(model_path_.c_str(), error, sizeof(error)); - if (!m_) { - mju_error("Load model failed: %s", error); - } + mjModel* MuJocoViewer::model() const { + return world_ ? world_->model() : nullptr; + } - d_ = mj_makeData(m_); - if (!d_) { - 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 lock(sim_->mtx); - mj_forward(m_, d_); - } - - physics_started_.store(true); - - // 3) 进入物理循环 - physicsLoop(); + mjData* MuJocoViewer::data() const { + return world_ ? world_->data() : nullptr; } void MuJocoViewer::getCameraState(double &distance, @@ -165,12 +98,12 @@ namespace cmvr { double azimuth, double elevation) { // 相机初始化：根据模型设置一个合理的 free camera - if (m_) { - mjv_defaultFreeCamera(m_, &cam_); + if (model()) { + mjv_defaultFreeCamera(model(), &cam_); // 以模型中心为观察点 - cam_.lookat[0] = m_->stat.center[0]; - cam_.lookat[1] = m_->stat.center[1]; - cam_.lookat[2] = m_->stat.center[2]; + cam_.lookat[0] = model()->stat.center[0]; + cam_.lookat[1] = model()->stat.center[1]; + 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_height_ = height > 0 ? height : 240; + pip_width_ = display_width > 0 ? display_width : 320; + 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)); - bottom = std::max(0, std::min(pip_bottom_, fb_height - 1)); - width = std::min(pip_width_, fb_width - left); - height = std::min(pip_height_, fb_height - bottom); + width = std::min(pip_width_, fb_width); + height = std::min(pip_height_, fb_height); + const int max_left = fb_width - width; + 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; - rect.width = width; - rect.height = height; - rect.left = left; - rect.bottom = bottom; + mjrRect display_rect; + display_rect.width = width; + display_rect.height = height; + display_rect.left = left; + display_rect.bottom = bottom; const std::unique_lock 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_); - mjr_render(rect, &pip_scene_, &sim_->platform_ui->mjr_context()); + mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_); + + 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 lock(pip_rgb_mtx_); - const int w = rect.width; - const int h = rect.height; + const int w = render_rect.width; + const int h = render_rect.height; if (w > 0 && h > 0) { pip_rgb_.resize(static_cast(3 * w * h)); pip_depth_.resize(static_cast(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(m_->vis.map.znear) * static_cast(m_->stat.extent); - const double zfar = static_cast(m_->vis.map.zfar) * static_cast(m_->stat.extent); + const double znear = static_cast(render_model->vis.map.znear) * + static_cast(render_model->stat.extent); + const double zfar = static_cast(render_model->vis.map.zfar) * + static_cast(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() { - using Clock = mj::Simulate::Clock; + void MuJocoViewer::syncThreadFunc() { + if (!world_->isLoaded()) { + mju_error("MuJocoViewer requires a loaded MujocoWorld"); + } - std::chrono::time_point syncCPU; - mjtNum syncSim = 0; + { + std::lock_guard 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 lock(sim_->mtx); - if (!m_ || !d_) continue; - - 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::lock_guard lock(world_->mutex()); + sim_->Sync(/*state_only=*/true); } + 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） - physics_thread_ = std::thread(&MuJocoViewer::physicsThreadFunc, this); + sync_thread_ = std::thread(&MuJocoViewer::syncThreadFunc, this); // 当前线程跑 UI + 渲染（阻塞） sim_->RenderLoop(); - // UI 退出后，通知 physics 线程结束 + // UI 退出后，通知同步线程结束 sim_->exitrequest.store(2); - if (physics_thread_.joinable()) { - physics_thread_.join(); + if (sync_thread_.joinable()) { + 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 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 world; + { + std::lock_guard 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 lock(mtx_); + running_ = false; + return false; + } + + auto viewer = std::make_unique(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 lock(mtx_); + viewer_ = std::move(viewer); + } + + CMVR_LOG(INFO) << "[MujocoViewerDevice] run UI on main thread, id=" << id_; + viewer_->run(); + + { + std::lock_guard 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 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& rgb, + std::vector& depth, + int& width, + int& height, + uint64_t& frame_id) const { + std::lock_guard 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 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 lock(mtx_); + running_ = false; + viewer_.reset(); + } + return true; + } } // namespace cmvr diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp index 09ced4f0..4cf3364e 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp @@ -1,270 +1,65 @@ -// -// Created by lgv on 11/25/25. -// +#include +#include +#include +#include +#include -#include "algorithms/controllers/pid/include/pid_controller.h" +#include -#include "gtest/gtest.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" -#include -#include -#include -#include +namespace { -#include "common/vision/matplotlibcpp.h" - - -namespace plt = matplotlibcpp; - - -using namespace cmvr; - -class MyViewer : public cmvr::MuJocoViewer { -public: - using MuJocoViewer::MuJocoViewer; - - // 对外接口：设置右臂关节目标角度（rad） - void moveJ(const std::array& 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 q_target_{}; // 目标关节角 - - // PID 控制器 - PidController pid_; - double dt_ = 0.0; // 仿真时间步长 - - // 日志：时间 & 每帧的 7 关节实际角 / 目标角 - std::vector time_log_; - std::vector> q_log_; - std::vector> 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(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 q_frame{}; - std::array 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) +std::filesystem::path findProjectRoot() { - if (time_log_.empty() || q_log_.empty() || q_des_log_.empty()) { - std::printf("[MyViewer] No log data, skip plotting.\n"); - return; + const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; + auto current = std::filesystem::current_path(); + while (!current.empty()) { + if (std::filesystem::exists(current / marker)) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; } - - const std::vector& t = time_log_; - - plt::figure(); - - for (int j = 0; j < kRightArmDOF; ++j) { - // 为第 j 个关节准备数据 - std::vector q_j(t.size()); - std::vector 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()); + return {}; } -// === gtest === -TEST(mujoco_viewer_test, test_view_and_pid) +std::string defaultModelPath() { + 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 q = { - M_PI/6, 1.26, M_PI/12, - M_PI/4, 0.22, 0.65, 0.66 - }; - viewer.moveJ(q); +TEST(MujocoViewerTest, ShowsUiWithMujocoWorld) +{ + const char* model_path_env = std::getenv("MUJOCO_VIEWER_TEST_MODEL"); + const std::string model_path = + model_path_env != nullptr && model_path_env[0] != '\0' + ? model_path_env + : defaultModelPath(); + auto world = std::make_shared(); + 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 个关节的响应曲线 - viewer.plotJointResponse("right_arm_joint_response.png"); + world->stop(); } - diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/platform_ui_adapter.cc b/cmvr-es/simulate/mujoco/mujoco_viewer/src/platform_ui_adapter.cc deleted file mode 100644 index e18d3f49..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/platform_ui_adapter.cc +++ /dev/null @@ -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 - -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( - 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; - } -} - -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(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 diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/simulate.cc b/cmvr-es/simulate/mujoco/mujoco_viewer/src/simulate.cc deleted file mode 100644 index 28aa9583..00000000 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/simulate.cc +++ /dev/null @@ -1,2982 +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. - -#include "../include/simulate.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include "simulate/mujoco/mujoco_viewer/include/array_safety.h" -#include "simulate/mujoco/mujoco_viewer/include/platform_ui_adapter.h" -#include "simulate/mujoco/mujoco_viewer/include/lodepng.h" - -// When launched via an App Bundle on macOS, the working directory is the path to the App Bundle's -// resource directory. This causes files to be saved into the bundle, which is not the desired -// behavior. Instead, we open a save dialog box to ask the user where to put the file. -// Since the dialog box logic needs to be written in Objective-C, we separate it into a different -// source file. -#ifdef __APPLE__ -std::string GetSavePath(const char* filename); -#else -static std::string GetSavePath(const char* filename) { - return filename; -} -#endif - -namespace { -namespace mj = ::mujoco; -namespace mju = ::mujoco::sample_util; - -using Seconds = std::chrono::duration; -using Milliseconds = std::chrono::duration; - -template -inline bool IsDifferent(const T& a, const T& b) { - if constexpr (std::is_array_v) { - static_assert(std::rank_v == 1); - for (int i = 0; i < std::extent_v; ++i) { - if (a[i] != b[i]) { - return true; - } - } - return false; - } else { - return a != b; - } -} - -template -inline void CopyScalar(T& dst, const T& src) { - dst = src; -} - -template -inline void CopyArray(T (&dst)[N], const T (&src)[N]) { - for (int i = 0; i < N; ++i) { - dst[i] = src[i]; - } -} - -template -inline void Copy(T& dst, const T& src) { - if constexpr (std::is_array_v) { - CopyArray(dst, src); - } else { - CopyScalar(dst, src); - } -} - -//------------------------------------------- global ----------------------------------------------- - -const double zoom_increment = 0.02; // ratio of one click-wheel zoom increment to vertical extent - -// section ids -enum { - // left ui - SECT_FILE = 0, - SECT_OPTION, - SECT_SIMULATION, - SECT_WATCH, - SECT_PHYSICS, - SECT_RENDERING, - SECT_VISUALIZATION, - SECT_GROUP, - NSECT0, - - // right ui - SECT_JOINT = 0, - SECT_CONTROL, - SECT_EQUALITY, - NSECT1 -}; - -// file section of UI -const mjuiDef defFile[] = { - {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"}, - {mjITEM_BUTTON, "Print data", 2, nullptr, "CD"}, - {mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"}, - {mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"}, - {mjITEM_END} -}; - -// help strings -const char help_content[] = - "Space\n" - "+ -\n" - "Left / Right arrow\n" - "Tab / Shift-Tab\n" - "[ ]\n" - "Esc\n" - "Double-click\n" - "Page Up\n" - "Right double-click\n" - "Ctrl Right double-click\n" - "Scroll, middle drag\n" - "Left drag\n" - "[Shift] right drag\n" - "Ctrl [Shift] drag\n" - "Ctrl [Shift] right drag\n" - "F1\n" - "F2\n" - "F3\n" - "F4\n" - "F5\n" - "UI right-button hold\n" - "UI title double-click"; - -const char help_title[] = - "Play / Pause\n" - "Speed Up / Down\n" - "Step Back / Forward\n" - "Toggle Left / Right UI\n" - "Cycle cameras\n" - "Free camera\n" - "Select\n" - "Select parent\n" - "Center camera\n" - "Tracking camera\n" - "Zoom\n" - "View Orbit\n" - "View Pan\n" - "Object Rotate\n" - "Object Translate\n" - "Help\n" - "Info\n" - "Profiler\n" - "Sensors\n" - "Full screen\n" - "Show UI shortcuts\n" - "Expand/collapse all"; - - -//-------------------------------- profiler, sensor, info, watch ----------------------------------- - -// number of lines in the Constraint ("Counts") and Cost ("Convergence") figures -static constexpr int kConstraintNum = 5; -static constexpr int kCostNum = 3; - -// init profiler figures -void InitializeProfiler(mj::Simulate* sim) { - // set figures to default - mjv_defaultFigure(&sim->figconstraint); - mjv_defaultFigure(&sim->figcost); - mjv_defaultFigure(&sim->figtimer); - mjv_defaultFigure(&sim->figsize); - - // titles - mju::strcpy_arr(sim->figconstraint.title, "Counts"); - mju::strcpy_arr(sim->figcost.title, "Convergence (log 10)"); - mju::strcpy_arr(sim->figsize.title, "Dimensions"); - mju::strcpy_arr(sim->figtimer.title, "CPU time (msec)"); - - // x-labels - mju::strcpy_arr(sim->figconstraint.xlabel, "Solver iteration"); - mju::strcpy_arr(sim->figcost.xlabel, "Solver iteration"); - mju::strcpy_arr(sim->figsize.xlabel, "Video frame"); - mju::strcpy_arr(sim->figtimer.xlabel, "Video frame"); - - // y-tick number formats - mju::strcpy_arr(sim->figconstraint.yformat, "%.0f"); - mju::strcpy_arr(sim->figcost.yformat, "%.1f"); - mju::strcpy_arr(sim->figsize.yformat, "%.0f"); - mju::strcpy_arr(sim->figtimer.yformat, "%.2f"); - - // colors - sim->figconstraint.figurergba[0] = 0.1f; - sim->figcost.figurergba[2] = 0.2f; - sim->figsize.figurergba[0] = 0.1f; - sim->figtimer.figurergba[2] = 0.2f; - sim->figconstraint.figurergba[3] = 0.5f; - sim->figcost.figurergba[3] = 0.5f; - sim->figsize.figurergba[3] = 0.5f; - sim->figtimer.figurergba[3] = 0.5f; - - // repeat line colors for constraint and cost figures - mjvFigure* fig = &sim->figcost; - for (int i=kCostNum; ilinergb[i][0] = fig->linergb[i - kCostNum][0]; - fig->linergb[i][1] = fig->linergb[i - kCostNum][1]; - fig->linergb[i][2] = fig->linergb[i - kCostNum][2]; - } - fig = &sim->figconstraint; - for (int i=kConstraintNum; ilinergb[i][0] = fig->linergb[i - kConstraintNum][0]; - fig->linergb[i][1] = fig->linergb[i - kConstraintNum][1]; - fig->linergb[i][2] = fig->linergb[i - kConstraintNum][2]; - } - - // legends - mju::strcpy_arr(sim->figconstraint.linename[0], "total"); - mju::strcpy_arr(sim->figconstraint.linename[1], "active"); - mju::strcpy_arr(sim->figconstraint.linename[2], "changed"); - mju::strcpy_arr(sim->figconstraint.linename[3], "evals"); - mju::strcpy_arr(sim->figconstraint.linename[4], "updates"); - mju::strcpy_arr(sim->figcost.linename[0], "improvement"); - mju::strcpy_arr(sim->figcost.linename[1], "gradient"); - mju::strcpy_arr(sim->figcost.linename[2], "lineslope"); - mju::strcpy_arr(sim->figsize.linename[0], "dof"); - mju::strcpy_arr(sim->figsize.linename[1], "body"); - mju::strcpy_arr(sim->figsize.linename[2], "constraint"); - mju::strcpy_arr(sim->figsize.linename[3], "sqrt(nnz)"); - mju::strcpy_arr(sim->figsize.linename[4], "contact"); - mju::strcpy_arr(sim->figsize.linename[5], "iteration"); - mju::strcpy_arr(sim->figtimer.linename[0], "total"); - mju::strcpy_arr(sim->figtimer.linename[1], "collision"); - mju::strcpy_arr(sim->figtimer.linename[2], "prepare"); - mju::strcpy_arr(sim->figtimer.linename[3], "solve"); - mju::strcpy_arr(sim->figtimer.linename[4], "other"); - - // grid sizes - sim->figconstraint.gridsize[0] = 5; - sim->figconstraint.gridsize[1] = 5; - sim->figcost.gridsize[0] = 5; - sim->figcost.gridsize[1] = 5; - sim->figsize.gridsize[0] = 3; - sim->figsize.gridsize[1] = 5; - sim->figtimer.gridsize[0] = 3; - sim->figtimer.gridsize[1] = 5; - - // minimum ranges - sim->figconstraint.range[0][0] = 0; - sim->figconstraint.range[0][1] = 20; - sim->figconstraint.range[1][0] = 0; - sim->figconstraint.range[1][1] = 80; - sim->figcost.range[0][0] = 0; - sim->figcost.range[0][1] = 20; - sim->figcost.range[1][0] = -15; - sim->figcost.range[1][1] = 5; - sim->figsize.range[0][0] = -200; - sim->figsize.range[0][1] = 0; - sim->figsize.range[1][0] = 0; - sim->figsize.range[1][1] = 100; - sim->figtimer.range[0][0] = -200; - sim->figtimer.range[0][1] = 0; - sim->figtimer.range[1][0] = 0; - sim->figtimer.range[1][1] = 0.4f; - - // init x axis on history figures (do not show yet) - for (int n=0; n<6; n++) { - for (int i=0; ifigtimer.linedata[n][2*i] = -i; - sim->figsize.linedata[n][2*i] = -i; - } - } -} - -// update profiler figures -void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) { - // reset lines in Constraint and Cost figures - memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int)); - memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int)); - - // number of islands that have diagnostics - int nisland = mjMAX(1, mjMIN(d->nisland, mjNISLAND)); - - // iterate over islands - for (int k=0; k < nisland; k++) { - // ==== update Constraint ("Counts") figure - - // number of points to plot, starting line - int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT); - int start = kConstraintNum * k; - - sim->figconstraint.linepnt[start + 0] = npoints; - for (int i=1; i < kConstraintNum; i++) { - sim->figconstraint.linepnt[start + i] = npoints; - } - if (m->opt.solver == mjSOL_PGS) { - sim->figconstraint.linepnt[start + 3] = 0; - sim->figconstraint.linepnt[start + 4] = 0; - } - if (m->opt.solver == mjSOL_CG) { - sim->figconstraint.linepnt[start + 4] = 0; - } - for (int i=0; ifigconstraint.linedata[start + 0][2*i] = i; - sim->figconstraint.linedata[start + 1][2*i] = i; - sim->figconstraint.linedata[start + 2][2*i] = i; - sim->figconstraint.linedata[start + 3][2*i] = i; - sim->figconstraint.linedata[start + 4][2*i] = i; - - // y - int nefc = nisland == 1 ? d->nefc : d->island_nefc[k]; - sim->figconstraint.linedata[start + 0][2*i+1] = nefc; - const mjSolverStat* stat = d->solver + k*mjNSOLVER + i; - sim->figconstraint.linedata[start + 1][2*i+1] = stat->nactive; - sim->figconstraint.linedata[start + 2][2*i+1] = stat->nchange; - sim->figconstraint.linedata[start + 3][2*i+1] = stat->neval; - sim->figconstraint.linedata[start + 4][2*i+1] = stat->nupdate; - } - - // update cost figure - start = kCostNum * k; - sim->figcost.linepnt[start + 0] = npoints; - for (int i=1; ifigcost.linepnt[start + i] = npoints; - } - if (m->opt.solver==mjSOL_PGS) { - sim->figcost.linepnt[start + 1] = 0; - sim->figcost.linepnt[start + 2] = 0; - } - - for (int i=0; ifigcost.linepnt[0]; i++) { - // x - sim->figcost.linedata[start + 0][2*i] = i; - sim->figcost.linedata[start + 1][2*i] = i; - sim->figcost.linedata[start + 2][2*i] = i; - - // y - const mjSolverStat* stat = d->solver + k*mjNSOLVER + i; - sim->figcost.linedata[start + 0][2*i + 1] = - mju_log10(mju_max(mjMINVAL, stat->improvement)); - sim->figcost.linedata[start + 1][2*i + 1] = - mju_log10(mju_max(mjMINVAL, stat->gradient)); - sim->figcost.linedata[start + 2][2*i + 1] = - mju_log10(mju_max(mjMINVAL, stat->lineslope)); - } - } - - // get timers: total, collision, prepare, solve, other - mjtNum total = d->timer[mjTIMER_STEP].duration; - int number = d->timer[mjTIMER_STEP].number; - if (!number) { - total = d->timer[mjTIMER_FORWARD].duration; - number = d->timer[mjTIMER_FORWARD].number; - } - - if (number) { // skip update if no measurements - float tdata[5] = { - static_cast(total/number), - static_cast(d->timer[mjTIMER_POS_COLLISION].duration/number), - static_cast(d->timer[mjTIMER_POS_MAKE].duration/number) + - static_cast(d->timer[mjTIMER_POS_PROJECT].duration/number), - static_cast(d->timer[mjTIMER_CONSTRAINT].duration/number), - 0 - }; - tdata[4] = tdata[0] - tdata[1] - tdata[2] - tdata[3]; - - // update figtimer - int pnt = mjMIN(201, sim->figtimer.linepnt[0]+1); - for (int n=0; n<5; n++) { - // shift data - for (int i=pnt-1; i>0; i--) { - sim->figtimer.linedata[n][2*i+1] = sim->figtimer.linedata[n][2*i-1]; - } - - // assign new - sim->figtimer.linepnt[n] = pnt; - sim->figtimer.linedata[n][1] = tdata[n]; - } - } - - // get total number of iterations and nonzeros - mjtNum sqrt_nnz = 0; - int solver_niter = 0; - for (int island=0; island < nisland; island++) { - sqrt_nnz += mju_sqrt(d->solver_nnz[island]); - solver_niter += d->solver_niter[island]; - } - - // get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter - float sdata[6] = { - static_cast(m->nv), - static_cast(m->nbody), - static_cast(d->nefc), - static_cast(sqrt_nnz), - static_cast(d->ncon), - static_cast(solver_niter) / nisland - }; - - // update figsize - int pnt = mjMIN(201, sim->figsize.linepnt[0]+1); - for (int n=0; n<6; n++) { - // shift data - for (int i=pnt-1; i>0; i--) { - sim->figsize.linedata[n][2*i+1] = sim->figsize.linedata[n][2*i-1]; - } - - // assign new - sim->figsize.linepnt[n] = pnt; - sim->figsize.linedata[n][1] = sdata[n]; - } -} - -// show profiler figures -void ShowProfiler(mj::Simulate* sim, mjrRect rect) { - mjrRect viewport = { - rect.left + rect.width - rect.width/4, - rect.bottom, - rect.width/4, - rect.height/4 - }; - mjr_figure(viewport, &sim->figtimer, &sim->platform_ui->mjr_context()); - viewport.bottom += rect.height/4; - mjr_figure(viewport, &sim->figsize, &sim->platform_ui->mjr_context()); - viewport.bottom += rect.height/4; - mjr_figure(viewport, &sim->figcost, &sim->platform_ui->mjr_context()); - viewport.bottom += rect.height/4; - mjr_figure(viewport, &sim->figconstraint, &sim->platform_ui->mjr_context()); -} - - -// init sensor figure -void InitializeSensor(mj::Simulate* sim) { - mjvFigure& figsensor = sim->figsensor; - - // set figure to default - mjv_defaultFigure(&figsensor); - figsensor.figurergba[3] = 0.5f; - - // set flags - figsensor.flg_extend = 1; - figsensor.flg_barplot = 1; - figsensor.flg_symmetric = 1; - - // title - mju::strcpy_arr(figsensor.title, "Sensor data"); - - // y-tick number format - mju::strcpy_arr(figsensor.yformat, "%.1f"); - - // grid size - figsensor.gridsize[0] = 2; - figsensor.gridsize[1] = 3; - - // minimum range - figsensor.range[0][0] = 0; - figsensor.range[0][1] = 0; - figsensor.range[1][0] = -1; - figsensor.range[1][1] = 1; -} - -// update sensor figure -void UpdateSensor(mj::Simulate* sim, const mjModel* m, const mjData* d) { - mjvFigure& figsensor = sim->figsensor; - static const int maxline = 10; - - // clear linepnt - for (int i=0; insensor; n++) { - // go to next line if type is different - if (n>0 && m->sensor_type[n]!=m->sensor_type[n-1]) { - lineid = mjMIN(lineid+1, maxline-1); - } - - // get info about this sensor - mjtNum cutoff = (m->sensor_cutoff[n]>0 ? m->sensor_cutoff[n] : 1); - int adr = m->sensor_adr[n]; - int dim = m->sensor_dim[n]; - - // data pointer in line - int p = figsensor.linepnt[lineid]; - - // fill in data for this sensor - for (int i=0; i=mjMAXLINEPNT/2) { - break; - } - - // x - figsensor.linedata[lineid][2*p+4*i] = adr+i; - figsensor.linedata[lineid][2*p+4*i+2] = adr+i; - - // y - figsensor.linedata[lineid][2*p+4*i+1] = 0; - figsensor.linedata[lineid][2*p+4*i+3] = d->sensordata[adr+i]/cutoff; - } - - // update linepnt - figsensor.linepnt[lineid] = mjMIN(mjMAXLINEPNT-1, figsensor.linepnt[lineid]+2*dim); - } -} - -// show sensor figure -void ShowSensor(mj::Simulate* sim, mjrRect rect) { - // constant width with and without profiler - int width = sim->profiler ? rect.width/3 : rect.width/4; - - // render figure on the right - mjrRect viewport = { - rect.left + rect.width - width, - rect.bottom, - width, - rect.height/3 - }; - mjr_figure(viewport, &sim->figsensor, &sim->platform_ui->mjr_context()); -} - -void ShowFigure(mj::Simulate* sim, mjrRect viewport, mjvFigure* fig){ - mjr_figure(viewport, fig, &sim->platform_ui->mjr_context()); -} - -void ShowOverlayText(mj::Simulate* sim, mjrRect viewport, int font, int gridpos, - std::string text1, std::string text2) { - mjr_overlay(font, gridpos, viewport, text1.c_str(), text2.c_str(), - &sim->platform_ui->mjr_context()); -} - -void ShowImage(mj::Simulate* sim, mjrRect viewport, const unsigned char* image) { - mjr_drawPixels(image, nullptr, viewport, &sim->platform_ui->mjr_context()); -} - -// load state from history buffer -static void LoadScrubState(mj::Simulate* sim) { - // get index into circular buffer - int i = (sim->scrub_index + sim->history_cursor_) % sim->nhistory_; - i = (i + sim->nhistory_) % sim->nhistory_; - - // load state - mjtNum* state = &sim->history_[i * sim->state_size_]; - mj_setState(sim->m_, sim->d_, state, mjSTATE_INTEGRATION); - - // call forward dynamics - mj_forward(sim->m_, sim->d_); -} - -// update an entire section of ui0 -static void mjui0_update_section(mj::Simulate* sim, int section) { - mjui_update(section, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context()); -} - -// prepare info text -void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d, - char (&title)[mj::Simulate::kMaxFilenameLength], - char (&content)[mj::Simulate::kMaxFilenameLength]) { - char tmp[20]; - - // number of islands with statistics - int nisland = mjMAX(1, mjMIN(d->nisland, mjNISLAND)); - - // compute solver error (maximum over islands) - mjtNum solerr = 0; - for (int i=0; i < nisland; i++) { - mjtNum solerr_i = 0; - if (d->solver_niter[i]) { - int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1; - const mjSolverStat* stat = d->solver + i*mjNSOLVER + ind; - solerr_i = mju_min(stat->improvement, stat->gradient); - if (solerr_i==0) { - solerr_i = mju_max(stat->improvement, stat->gradient); - } - } - solerr = mju_max(solerr, solerr_i); - } - solerr = mju_log10(mju_max(mjMINVAL, solerr)); - - // format FPS text - char fps[10]; - if (sim->fps_ < 1) { - mju::sprintf_arr(fps, "%0.1f ", sim->fps_); - } else { - mju::sprintf_arr(fps, "%.0f ", sim->fps_); - } - - // total iterations of all islands with statistics - int solver_niter = 0; - for (int i=0; i < nisland; i++) { - solver_niter += d->solver_niter[i]; - } - - // 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%.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, - 100*d->maxuse_arena/(double)(d->narena), - mju_writeNumBytes(d->narena)); - - // add Energy if enabled - { - if (mjENABLED(mjENBL_ENERGY)) { - mju::sprintf_arr(tmp, "\n%.3f", d->energy[0]+d->energy[1]); - mju::strcat_arr(content, tmp); - mju::strcat_arr(title, "\nEnergy"); - } - - // add FwdInv if enabled - if (mjENABLED(mjENBL_FWDINV)) { - mju::sprintf_arr(tmp, "\n%.1f %.1f", - mju_log10(mju_max(mjMINVAL, d->solver_fwdinv[0])), - mju_log10(mju_max(mjMINVAL, d->solver_fwdinv[1]))); - mju::strcat_arr(content, tmp); - mju::strcat_arr(title, "\nFwdInv"); - } - - // add islands if enabled - if (!mjDISABLED(mjDSBL_ISLAND) && d->nisland > 0) { - mju::sprintf_arr(tmp, "\n%d", d->nisland); - mju::strcat_arr(content, tmp); - mju::strcat_arr(title, "\nIslands"); - } - } -} - -// sprintf forwarding, to avoid compiler warning in x-macro -void PrintField(char (&str)[mjMAXUINAME], void* ptr) { - mju::sprintf_arr(str, "%g", *static_cast(ptr)); -} - -// update watch -void UpdateWatch(mj::Simulate* sim, const mjModel* m, const mjData* d) { - // clear - sim->ui0.sect[SECT_WATCH].item[2].multi.nelem = 1; - mju::strcpy_arr(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid field"); - - // prepare symbols needed by xmacro - MJDATA_POINTERS_PREAMBLE(m); - - // find specified field in mjData arrays, update value - #define X(TYPE, NAME, NR, NC) \ - if (!mju::strcmp_arr(#NAME, sim->field) && \ - !mju::strcmp_arr(#TYPE, "mjtNum")) { \ - if (sim->index >= 0 && sim->index < m->NR * NC) { \ - PrintField(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], d->NAME + sim->index); \ - } else { \ - mju::strcpy_arr(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid index"); \ - } \ - return; \ - } - - MJDATA_POINTERS -#undef X -} - - -//---------------------------------- UI construction ----------------------------------------------- - -// make physics section of UI -void MakePhysicsSection(mj::Simulate* sim) { - mjOption* opt = sim->is_passive_ ? &sim->m_passive_->opt : &sim->m_->opt; - mjuiDef defPhysics[] = { - {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", 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"}, - {mjITEM_EDITINT, "LS Iter", 2, &(opt->ls_iterations), "1 0 100"}, - {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, "CCD Iter", 2, &(opt->ccd_iterations), "1 0 1000"}, - {mjITEM_EDITNUM, "CCD Tol", 2, &(opt->ccd_tolerance), "1 0 1"}, - {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", 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", mjPRESERVE}, - {mjITEM_END} - }; - mjuiDef defEnableFlags[] = { - {mjITEM_SEPARATOR, "Enable Flags", mjPRESERVE}, - {mjITEM_END} - }; - mjuiDef defOverride[] = { - {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", 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 - mjui_add(&sim->ui0, defPhysics); - - // add flags programmatically - mjuiDef defFlag[] = { - {mjITEM_CHECKINT, "", 2, nullptr, ""}, - {mjITEM_END} - }; - for (int i=0; idisable + i; - mjui_add(&sim->ui0, defFlag); - } - mjui_add(&sim->ui0, defEnableFlags); - for (int i=0; ienable + i; - mjui_add(&sim->ui0, defFlag); - } - // add contact override - mjui_add(&sim->ui0, defOverride); - - // 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) { - mjuiDef defRendering[] = { - {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), - "None\nBody\nGeom\nSite\nCamera\nLight\nContact\nWorld" - }, - {mjITEM_BUTTON, "Copy camera", 2, nullptr, ""}, - {mjITEM_SEPARATOR, "Model Elements", 1}, - {mjITEM_END} - }; - mjuiDef defOpenGL[] = { - {mjITEM_SEPARATOR, "OpenGL Effects", 1}, - {mjITEM_END} - }; - - // add model cameras, up to UI limit - for (int i=0; incam, mjMAXUIMULTI-2); i++) { - // prepare name - char camname[mjMAXUINAME] = "\n"; - if (m->names[m->name_camadr[i]]) { - mju::strcat_arr(camname, m->names+m->name_camadr[i]); - } else { - mju::sprintf_arr(camname, "\nCamera %d", i); - } - - // check string length - if (mju::strlen_arr(camname) + mju::strlen_arr(defRendering[1].other)>=mjMAXUITEXT-1) { - break; - } - - // add camera - mju::strcat_arr(defRendering[1].other, camname); - } - - // add rendering standard - mjui_add(&sim->ui0, defRendering); - - // add flags programmatically - mjuiDef defFlag[] = { - {mjITEM_CHECKBYTE, "", 2, nullptr, ""}, - {mjITEM_END} - }; - for (int i=0; iopt.flags + i; - mjui_add(&sim->ui0, defFlag); - } - - // create tree slider - mjuiDef defTree[] = { - {mjITEM_SLIDERINT, "Tree depth", 2, &sim->opt.bvh_depth, "0 20"}, - {mjITEM_SLIDERINT, "Flex layer", 2, &sim->opt.flex_layer, "0 10"}, - {mjITEM_END} - }; - mjui_add(&sim->ui0, defTree); - - // add rendering flags - mjui_add(&sim->ui0, defOpenGL); - for (int i=0; iscn.flags + i; - mjui_add(&sim->ui0, defFlag); - } -} - -// make visualization section of UI -void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) { - mjStatistic* stat = sim->is_passive_ ? &sim->m_passive_->stat : &sim->m_->stat; - mjVisual* vis = sim->is_passive_ ? &sim->m_passive_->vis : &sim->m_->vis; - - mjuiDef defVisualization[] = { - {mjITEM_SECTION, "Visualization", mjPRESERVE, nullptr, "AV"}, - {mjITEM_SEPARATOR, "Headlight", 1}, - {mjITEM_RADIO, "Active", 2, &(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, "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_RADIO, "Inertia", 2, &(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"}, - {mjITEM_EDITFLOAT, "Force", 2, &(vis->map.force), "1"}, - {mjITEM_EDITFLOAT, "Torque", 2, &(vis->map.torque), "1"}, - {mjITEM_EDITFLOAT, "Alpha", 2, &(vis->map.alpha), "1"}, - {mjITEM_EDITFLOAT, "Fog start", 2, &(vis->map.fogstart), "1"}, - {mjITEM_EDITFLOAT, "Fog end", 2, &(vis->map.fogend), "1"}, - {mjITEM_EDITFLOAT, "Z near", 2, &(vis->map.znear), "1"}, - {mjITEM_EDITFLOAT, "Z far", 2, &(vis->map.zfar), "1"}, - {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", 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"}, - {mjITEM_EDITFLOAT, "Connect", 2, &(vis->scale.connect), "1"}, - {mjITEM_EDITFLOAT, "Com", 2, &(vis->scale.com), "1"}, - {mjITEM_EDITFLOAT, "Camera", 2, &(vis->scale.camera), "1"}, - {mjITEM_EDITFLOAT, "Light", 2, &(vis->scale.light), "1"}, - {mjITEM_EDITFLOAT, "Select point", 2, &(vis->scale.selectpoint), "1"}, - {mjITEM_EDITFLOAT, "Joint length", 2, &(vis->scale.jointlength), "1"}, - {mjITEM_EDITFLOAT, "Joint width", 2, &(vis->scale.jointwidth), "1"}, - {mjITEM_EDITFLOAT, "Actuator length", 2, &(vis->scale.actuatorlength), "1"}, - {mjITEM_EDITFLOAT, "Actuator width", 2, &(vis->scale.actuatorwidth), "1"}, - {mjITEM_EDITFLOAT, "Frame length", 2, &(vis->scale.framelength), "1"}, - {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 visualization section - mjui_add(&sim->ui0, defVisualization); -} - -// make group section of UI -void MakeGroupSection(mj::Simulate* sim) { - mjuiDef defGroup[] = { - {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"}, - {mjITEM_CHECKBYTE, "Geom 2", 2, sim->opt.geomgroup+2, " 2"}, - {mjITEM_CHECKBYTE, "Geom 3", 2, sim->opt.geomgroup+3, " 3"}, - {mjITEM_CHECKBYTE, "Geom 4", 2, sim->opt.geomgroup+4, " 4"}, - {mjITEM_CHECKBYTE, "Geom 5", 2, sim->opt.geomgroup+5, " 5"}, - {mjITEM_SEPARATOR, "Site groups", 1}, - {mjITEM_CHECKBYTE, "Site 0", 2, sim->opt.sitegroup, "S0"}, - {mjITEM_CHECKBYTE, "Site 1", 2, sim->opt.sitegroup+1, "S1"}, - {mjITEM_CHECKBYTE, "Site 2", 2, sim->opt.sitegroup+2, "S2"}, - {mjITEM_CHECKBYTE, "Site 3", 2, sim->opt.sitegroup+3, "S3"}, - {mjITEM_CHECKBYTE, "Site 4", 2, sim->opt.sitegroup+4, "S4"}, - {mjITEM_CHECKBYTE, "Site 5", 2, sim->opt.sitegroup+5, "S5"}, - {mjITEM_SEPARATOR, "Joint groups", 1}, - {mjITEM_CHECKBYTE, "Joint 0", 2, sim->opt.jointgroup, ""}, - {mjITEM_CHECKBYTE, "Joint 1", 2, sim->opt.jointgroup+1, ""}, - {mjITEM_CHECKBYTE, "Joint 2", 2, sim->opt.jointgroup+2, ""}, - {mjITEM_CHECKBYTE, "Joint 3", 2, sim->opt.jointgroup+3, ""}, - {mjITEM_CHECKBYTE, "Joint 4", 2, sim->opt.jointgroup+4, ""}, - {mjITEM_CHECKBYTE, "Joint 5", 2, sim->opt.jointgroup+5, ""}, - {mjITEM_SEPARATOR, "Tendon groups", 1}, - {mjITEM_CHECKBYTE, "Tendon 0", 2, sim->opt.tendongroup, ""}, - {mjITEM_CHECKBYTE, "Tendon 1", 2, sim->opt.tendongroup+1, ""}, - {mjITEM_CHECKBYTE, "Tendon 2", 2, sim->opt.tendongroup+2, ""}, - {mjITEM_CHECKBYTE, "Tendon 3", 2, sim->opt.tendongroup+3, ""}, - {mjITEM_CHECKBYTE, "Tendon 4", 2, sim->opt.tendongroup+4, ""}, - {mjITEM_CHECKBYTE, "Tendon 5", 2, sim->opt.tendongroup+5, ""}, - {mjITEM_SEPARATOR, "Actuator groups", 1}, - {mjITEM_CHECKBYTE, "Actuator 0", 2, sim->opt.actuatorgroup, ""}, - {mjITEM_CHECKBYTE, "Actuator 1", 2, sim->opt.actuatorgroup+1, ""}, - {mjITEM_CHECKBYTE, "Actuator 2", 2, sim->opt.actuatorgroup+2, ""}, - {mjITEM_CHECKBYTE, "Actuator 3", 2, sim->opt.actuatorgroup+3, ""}, - {mjITEM_CHECKBYTE, "Actuator 4", 2, sim->opt.actuatorgroup+4, ""}, - {mjITEM_CHECKBYTE, "Actuator 5", 2, sim->opt.actuatorgroup+5, ""}, - {mjITEM_SEPARATOR, "Flex groups", 1}, - {mjITEM_CHECKBYTE, "Flex 0", 2, sim->opt.flexgroup, ""}, - {mjITEM_CHECKBYTE, "Flex 1", 2, sim->opt.flexgroup+1, ""}, - {mjITEM_CHECKBYTE, "Flex 2", 2, sim->opt.flexgroup+2, ""}, - {mjITEM_CHECKBYTE, "Flex 3", 2, sim->opt.flexgroup+3, ""}, - {mjITEM_CHECKBYTE, "Flex 4", 2, sim->opt.flexgroup+4, ""}, - {mjITEM_CHECKBYTE, "Flex 5", 2, sim->opt.flexgroup+5, ""}, - {mjITEM_SEPARATOR, "Skin groups", 1}, - {mjITEM_CHECKBYTE, "Skin 0", 2, sim->opt.skingroup, ""}, - {mjITEM_CHECKBYTE, "Skin 1", 2, sim->opt.skingroup+1, ""}, - {mjITEM_CHECKBYTE, "Skin 2", 2, sim->opt.skingroup+2, ""}, - {mjITEM_CHECKBYTE, "Skin 3", 2, sim->opt.skingroup+3, ""}, - {mjITEM_CHECKBYTE, "Skin 4", 2, sim->opt.skingroup+4, ""}, - {mjITEM_CHECKBYTE, "Skin 5", 2, sim->opt.skingroup+5, ""}, - {mjITEM_END} - }; - - // add section - mjui_add(&sim->ui0, defGroup); -} - -// make joint section of UI -void MakeJointSection(mj::Simulate* sim) { - mjuiDef defJoint[] = { - {mjITEM_SECTION, "Joint", mjPRESERVE, nullptr, "AJ"}, - {mjITEM_END} - }; - mjuiDef defSlider[] = { - {mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"}, - {mjITEM_END} - }; - - // add section - mjui_add(&sim->ui1, defJoint); - defSlider[0].state = 4; - - // add scalar joints, exit if UI limit reached - int itemcnt = 0; - for (int i=0; i < sim->jnt_type_.size() && itemcntjnt_type_[i]==mjJNT_HINGE || sim->jnt_type_[i]==mjJNT_SLIDE)) { - // skip if joint group is disabled - if (!sim->opt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->jnt_group_[i]))]) { - continue; - } - - // set data and name - if (!sim->is_passive_) { - defSlider[0].pdata = &sim->d_->qpos[sim->m_->jnt_qposadr[i]]; - } else { - defSlider[0].pdata = &sim->qpos_[sim->jnt_qposadr_[i]]; - } - if (!sim->jnt_names_[i].empty()) { - mju::strcpy_arr(defSlider[0].name, sim->jnt_names_[i].c_str()); - } else { - mju::sprintf_arr(defSlider[0].name, "joint %d", i); - } - - // set range - if (sim->jnt_range_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", - sim->jnt_range_[i]->first, sim->jnt_range_[i]->second); - else if (sim->jnt_type_[i]==mjJNT_SLIDE) { - mju::strcpy_arr(defSlider[0].other, "-1 1"); - } else { - mju::strcpy_arr(defSlider[0].other, "-3.1416 3.1416"); - } - - // add and count - mjui_add(&sim->ui1, defSlider); - itemcnt++; - } - } -} - -// make control section of UI -void MakeControlSection(mj::Simulate* sim) { - mjuiDef defControl[] = { - {mjITEM_SECTION, "Control", mjPRESERVE, nullptr, "AC"}, - {mjITEM_BUTTON, "Clear all", 2}, - {mjITEM_END} - }; - mjuiDef defSlider[] = { - {mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"}, - {mjITEM_END} - }; - - // add section - mjui_add(&sim->ui1, defControl); - - // add controls, exit if UI limit reached (Clear button already added) - int itemcnt = 1; - for (int i=0; i < sim->actuator_ctrlrange_.size() && itemcntactuator_group_[i]; - if (!sim->opt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, group))]) { - continue; - } - // grey out if actuator group is disabled - if (group >= 0 && group <= 30 && sim->m_->opt.disableactuator & (1 << group)) { - defSlider[0].state = 0; - } else { - defSlider[0].state = 2; - } - - // set data and name - if (!sim->is_passive_) { - defSlider[0].pdata = &sim->d_->ctrl[i]; - } else { - defSlider[0].pdata = &sim->ctrl_[i]; - } - if (!sim->actuator_names_[i].empty()) { - mju::strcpy_arr(defSlider[0].name, sim->actuator_names_[i].c_str()); - } else { - mju::sprintf_arr(defSlider[0].name, "control %d", i); - } - - // set range - if (sim->actuator_ctrlrange_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", - sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second); - else { - mju::strcpy_arr(defSlider[0].other, "-1 1"); - } - - // add and count - mjui_add(&sim->ui1, defSlider); - itemcnt++; - } -} - -// make equality section of UI -void MakeEqualitySection(mj::Simulate* sim) { - mjuiDef defEquality[] = { - {mjITEM_SECTION, "Equality", mjPRESERVE, nullptr, "AE"}, - {mjITEM_END} - }; - mjuiDef defCheckBox[] = { - {mjITEM_CHECKBYTE, "", 2, nullptr, ""}, - {mjITEM_END} - }; - - // add section - mjui_add(&sim->ui1, defEquality); - - // add equalities, exit if UI limit reached - for (int i= 0; i < sim->m_->neq && id_->eq_active[i]; - - // set name - if (!sim->equality_names_[i].empty()) { - mju::strcpy_arr(defCheckBox[0].name, sim->equality_names_[i].c_str()); - } else { - mju::sprintf_arr(defCheckBox[0].name, "equality %d", i); - } - - mjui_add(&sim->ui1, defCheckBox); - } -} - -// make model-dependent UI sections -void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) { - // clear model-dependent sections of UI - sim->ui0.nsect = SECT_PHYSICS; - sim->ui1.nsect = 0; - - // make - MakePhysicsSection(sim); - MakeRenderingSection(sim, m); - MakeVisualizationSection(sim, m); - MakeGroupSection(sim); - MakeJointSection(sim); - MakeControlSection(sim); - MakeEqualitySection(sim); -} - -//---------------------------------- utility functions --------------------------------------------- - -// align and scale view -void AlignAndScaleView(mj::Simulate* sim, const mjModel* m) { - // if the id is valid, use the initial fixed camera - if (m->vis.global.cameraid >= 0 && m->vis.global.cameraid < m->ncam) { - sim->cam.fixedcamid = m->vis.global.cameraid; - sim->cam.type = mjCAMERA_FIXED; - } - - // otherwise use default free camera - else { - mjv_defaultFreeCamera(m, &sim->cam); - } -} - - -// copy state to clipboard as key -void CopyKey(mj::Simulate* sim, const mjModel* m, const mjData* d, bool fp) { - char clipboard[5000] = "time); - mju::strcat_arr(clipboard, buf); - - // qpos - mju::strcat_arr(clipboard, "\"\n qpos=\""); - for (int i = 0; i < m->nq; i++) { - mju::sprintf_arr(buf, format, d->qpos[i]); - if (i < m->nq-1) mju::strcat_arr(buf, " "); - mju::strcat_arr(clipboard, buf); - } - - // qvel - mju::strcat_arr(clipboard, "\"\n qvel=\""); - for (int i = 0; i < m->nv; i++) { - mju::sprintf_arr(buf, format, d->qvel[i]); - if (i < m->nv-1) mju::strcat_arr(buf, " "); - mju::strcat_arr(clipboard, buf); - } - - // act - if (m->na > 0) { - mju::strcat_arr(clipboard, "\"\n act=\""); - for (int i = 0; i < m->na; i++) { - mju::sprintf_arr(buf, format, d->act[i]); - if (i < m->na-1) mju::strcat_arr(buf, " "); - mju::strcat_arr(clipboard, buf); - } - } - - // ctrl - if (m->nu > 0) { - mju::strcat_arr(clipboard, "\"\n ctrl=\""); - for (int i = 0; i < m->nu; i++) { - mju::sprintf_arr(buf, format, d->ctrl[i]); - if (i < m->nu-1) mju::strcat_arr(buf, " "); - mju::strcat_arr(clipboard, buf); - } - } - - if (m->nmocap > 0) { - // mocap_pos - mju::strcat_arr(clipboard, "\"\n mpos=\""); - for (int i = 0; i < 3*m->nmocap; i++) { - mju::sprintf_arr(buf, format, d->mocap_pos[i]); - if (i < 3*m->nmocap-1) mju::strcat_arr(buf, " "); - mju::strcat_arr(clipboard, buf); - } - - // mocap_quat - mju::strcat_arr(clipboard, "\"\n mquat=\""); - for (int i = 0; i < 4*m->nmocap; i++) { - mju::sprintf_arr(buf, format, d->mocap_quat[i]); - if (i < 4*m->nmocap-1) mju::strcat_arr(buf, " "); - mju::strcat_arr(clipboard, buf); - } - } - - mju::strcat_arr(clipboard, "\"\n/>"); - - // copy to clipboard - sim->platform_ui->SetClipboardString(clipboard); -} - -// millisecond timer, for MuJoCo built-in profiler -mjtNum Timer() { - static auto start = mj::Simulate::Clock::now(); - auto elapsed = Milliseconds(mj::Simulate::Clock::now() - start); - return elapsed.count(); -} - -// clear all times -void ClearTimers(mjData* d) { - for (int i=0; itimer[i].duration = 0; - d->timer[i].number = 0; - } -} - -// copy current camera to clipboard as MJCF specification -void CopyCamera(mj::Simulate* sim) { - mjvGLCamera* camera = sim->scn.camera; - - char clipboard[500]; - mjtNum cam_right[3]; - mjtNum cam_forward[3]; - mjtNum cam_up[3]; - - // get camera spec from the GLCamera - mju_f2n(cam_forward, camera[0].forward, 3); - mju_f2n(cam_up, camera[0].up, 3); - mju_cross(cam_right, cam_forward, cam_up); - - // make MJCF camera spec - mju::sprintf_arr(clipboard, - "\n", - (camera[0].pos[0] + camera[1].pos[0]) / 2, - (camera[0].pos[1] + camera[1].pos[1]) / 2, - (camera[0].pos[2] + camera[1].pos[2]) / 2, - cam_right[0], cam_right[1], cam_right[2], - camera[0].up[0], camera[0].up[1], camera[0].up[2]); - - // copy spec into clipboard - sim->platform_ui->SetClipboardString(clipboard); -} - -// update UI 0 when MuJoCo structures change (except for joint sliders) -void UpdateSettings(mj::Simulate* sim, const mjModel* m) { - // physics flags - for (int i=0; iopt.disableflags & (1<disable[i] != new_value) { - sim->disable[i] = new_value; - sim->pending_.ui_update_physics = true; - } - } - for (int i=0; iopt.enableflags & (1<enable[i] != new_value) { - sim->enable[i] = new_value; - sim->pending_.ui_update_physics = true; - } - } - for (int i=0; iopt.disableactuator & (1<enableactuator[i] != enabled) { - sim->enableactuator[i] = enabled; - sim->pending_.ui_update_physics = true; - sim->pending_.ui_remake_ctrl = true; - } - } - - // camera - int old_camera = sim->camera; - if (sim->cam.type==mjCAMERA_FIXED) { - sim->camera = 2 + sim->cam.fixedcamid; - } else if (sim->cam.type==mjCAMERA_TRACKING) { - sim->camera = 1; - } else { - sim->camera = 0; - } - if (old_camera != sim->camera) { - sim->pending_.ui_update_rendering = true; - } -} - -// Compute suitable font scale. -int ComputeFontScale(const mj::PlatformUIAdapter& platform_ui) { - // compute framebuffer-to-window ratio - auto [buf_width, buf_height] = platform_ui.GetFramebufferSize(); - auto [win_width, win_height] = platform_ui.GetWindowSize(); - double b2w = static_cast(buf_width) / win_width; - - // compute PPI - double PPI = b2w * platform_ui.GetDisplayPixelsPerInch(); - - // estimate font scaling, guard against unrealistic PPI - int fs; - if (buf_width > win_width) { - fs = mju_round(b2w * 100); - } else if (PPI>50 && PPI<350) { - fs = mju_round(PPI); - } else { - fs = 150; - } - fs = mju_round(fs * 0.02) * 50; - fs = mjMIN(300, mjMAX(100, fs)); - - return fs; -} - - -//---------------------------------- UI handlers --------------------------------------------------- - -// determine enable/disable item state given category -int UiPredicate(int category, void* userdata) { - mj::Simulate* sim = static_cast(userdata); - - switch (category) { - case 2: // require model - return sim->m_ || sim->is_passive_; - - case 3: // require model and nkey - return (sim->m_ || sim->is_passive_) && sim->nkey_; - - case 4: // require model and paused - return sim->m_ && !sim->run; - - case 5: // require model and fully managed mode - return !sim->is_passive_ && sim->m_; - - default: - return 1; - } -} - -// set window layout -void UiLayout(mjuiState* state) { - mj::Simulate* sim = static_cast(state->userdata); - mjrRect* rect = state->rect; - - // set number of rectangles - state->nrect = 4; - - // rect 1: UI 0 - rect[1].left = 0; - rect[1].width = sim->ui0_enable ? sim->ui0.width : 0; - rect[1].bottom = 0; - rect[1].height = rect[0].height; - - // rect 2: UI 1 - rect[2].width = sim->ui1_enable ? sim->ui1.width : 0; - rect[2].left = mjMAX(0, rect[0].width - rect[2].width); - rect[2].bottom = 0; - rect[2].height = rect[0].height; - - // rect 3: 3D plot (everything else is an overlay) - rect[3].left = rect[1].width; - rect[3].width = mjMAX(0, rect[0].width - rect[1].width - rect[2].width); - rect[3].bottom = 0; - rect[3].height = rect[0].height; -} - -// modify UI -void UiModify(mjUI* ui, mjuiState* state, mjrContext* con) { - mjui_resize(ui, 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); -} - -// handle UI event -void UiEvent(mjuiState* state) { - mj::Simulate* sim = static_cast(state->userdata); - - // call UI 0 if event is directed to it - if ((state->dragrect==sim->ui0.rectid) || - (state->dragrect==0 && state->mouserect==sim->ui0.rectid) || - state->type==mjEVENT_KEY) { - // process UI event - mjuiItem* it = mjui_event(&sim->ui0, state, &sim->platform_ui->mjr_context()); - - // file section - if (it && it->sectionid==SECT_FILE) { - switch (it->itemid) { - case 0: // Save xml - sim->pending_.save_xml = GetSavePath("mjmodel.xml"); - break; - - case 1: // Save mjb - sim->pending_.save_mjb = GetSavePath("mjmodel.mjb"); - break; - - case 2: // Print model - sim->pending_.print_model = GetSavePath("MJMODEL.TXT"); - break; - - case 3: // Print data - sim->pending_.print_data = GetSavePath("MJDATA.TXT"); - break; - - case 4: // Quit - sim->exitrequest.store(1); - break; - - case 5: // Screenshot - sim->screenshotrequest.store(true); - break; - } - } - - // option section - else if (it && it->sectionid==SECT_OPTION) { - if (it->pdata == &sim->spacing) { - sim->ui0.spacing = mjui_themeSpacing(sim->spacing); - sim->ui1.spacing = mjui_themeSpacing(sim->spacing); - } else if (it->pdata == &sim->color) { - sim->ui0.color = mjui_themeColor(sim->color); - sim->ui1.color = mjui_themeColor(sim->color); - } else if (it->pdata == &sim->font) { - mjr_changeFont(50*(sim->font+1), &sim->platform_ui->mjr_context()); - } else if (it->pdata == &sim->fullscreen) { - sim->platform_ui->ToggleFullscreen(); - } else if (it->pdata == &sim->vsync) { - sim->platform_ui->SetVSync(sim->vsync); - } - - // modify UI - UiModify(&sim->ui0, state, &sim->platform_ui->mjr_context()); - UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); - } - - // simulation section - else if (it && it->sectionid==SECT_SIMULATION) { - switch (it->itemid) { - case 1: // Reset - sim->pending_.reset = true; - break; - - case 2: // Reload - sim->uiloadrequest.fetch_add(1); - break; - - case 3: // Align - sim->pending_.align = true; - break; - - case 4: // Copy key - sim->pending_.copy_key = true; - sim->pending_.copy_key_full_precision = sim->platform_ui->IsShiftKeyPressed(); - break; - - case 5: // Adjust key - case 6: // Load key - sim->pending_.load_key = true; - break; - - case 7: // Save key - sim->pending_.save_key = true; - break; - - case 11: // History scrubber - sim->run = 0; - sim->pending_.load_from_history = true; - mjui0_update_section(sim, SECT_SIMULATION); - break; - } - } - - // physics section - else if (it && it->sectionid==SECT_PHYSICS && sim->m_) { - mjOption* opt = sim->is_passive_ ? &sim->m_passive_->opt : &sim->m_->opt; - - // update disable flags in mjOption - opt->disableflags = 0; - for (int i=0; idisable[i]) { - opt->disableflags |= (1<enableflags = 0; - for (int i=0; ienable[i]) { - opt->enableflags |= (1<enableactuator[i]) != (opt->disableactuator & (1<enableactuator[i]) { - // disable actuator group i - opt->disableactuator |= (1<disableactuator &= ~(1<pending_.ui_remake_ctrl = true; - } - } - - // rendering section - else if (it && it->sectionid==SECT_RENDERING) { - // set camera in mjvCamera - if (sim->camera==0) { - sim->cam.type = mjCAMERA_FREE; - } else if (sim->camera==1) { - if (sim->pert.select>0) { - sim->cam.type = mjCAMERA_TRACKING; - sim->cam.trackbodyid = sim->pert.select; - sim->cam.fixedcamid = -1; - } else { - sim->cam.type = mjCAMERA_FREE; - sim->camera = 0; - mjui0_update_section(sim, SECT_RENDERING); - } - } else { - sim->cam.type = mjCAMERA_FIXED; - sim->cam.fixedcamid = sim->camera - 2; - } - // copy camera spec to clipboard (as MJCF element) - if (it->itemid == 3) { - CopyCamera(sim); - } - } - - // visualization section - else if (it && it->sectionid==SECT_VISUALIZATION) { - if (!mju::strcmp_arr(it->name, "Align")) { - sim->pending_.align = true; - } - } - - // group section - else if (it && it->sectionid==SECT_GROUP) { - // remake joint section if joint group changed - if (it->name[0]=='J' && it->name[1]=='o') { - sim->ui1.nsect = SECT_JOINT; - MakeJointSection(sim); - sim->ui1.nsect = NSECT1; - UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); - } - - // remake control section if actuator group changed - if (it->name[0]=='A' && it->name[1]=='c') { - sim->pending_.ui_remake_ctrl = true; - } - } - - // stop if UI processed event - if (it!=nullptr || (state->type==mjEVENT_KEY && state->key==0)) { - return; - } - } - - // call UI 1 if event is directed to it - if ((state->dragrect==sim->ui1.rectid) || - (state->dragrect==0 && state->mouserect==sim->ui1.rectid) || - state->type==mjEVENT_KEY) { - // process UI event - mjuiItem* it = mjui_event(&sim->ui1, state, &sim->platform_ui->mjr_context()); - - // control section - if (it && it->sectionid==SECT_CONTROL) { - // clear controls - if (it->itemid==0) { - sim->pending_.zero_ctrl = true; - } - } - - // stop if UI processed event - if (it!=nullptr || (state->type==mjEVENT_KEY && state->key==0)) { - return; - } - } - - // shortcut not handled by UI - if (state->type==mjEVENT_KEY && state->key!=0) { - switch (state->key) { - case ' ': // Mode - if (!sim->is_passive_ && sim->m_) { - sim->run = 1 - sim->run; - sim->pert.active = 0; - - if (sim->run) sim->scrub_index = 0; // reset scrubber - - mjui0_update_section(sim, -1); - } - break; - - case mjKEY_RIGHT: // step forward - if (!sim->is_passive_ && sim->m_ && !sim->run) { - ClearTimers(sim->d_); - - // currently in scrubber: increment scrub, load state, update slider UI - if (sim->scrub_index < 0) { - sim->scrub_index++; - sim->pending_.load_from_history = true; - mjui0_update_section(sim, SECT_SIMULATION); - } - - // not in scrubber: step, add to history buffer - else { - mj_step(sim->m_, sim->d_); - sim->AddToHistory(); - } - - UpdateProfiler(sim, sim->m_, sim->d_); - UpdateSensor(sim, sim->m_, sim->d_); - UpdateSettings(sim, sim->m_); - } - break; - - case mjKEY_LEFT: // step backward - if (!sim->is_passive_ && sim->m_) { - sim->run = 0; - ClearTimers(sim->d_); - - // decrement scrub, load state - sim->scrub_index = mjMAX(sim->scrub_index - 1, 1 - sim->nhistory_); - sim->pending_.load_from_history = true; - - // update slider UI, profiler, sensor - mjui0_update_section(sim, SECT_SIMULATION); - UpdateProfiler(sim, sim->m_, sim->d_); - UpdateSensor(sim, sim->m_, sim->d_); - } - break; - - case mjKEY_PAGE_UP: // select parent body - if ((sim->m_ || sim->is_passive_) && sim->pert.select > 0) { - sim->pert.select = sim->body_parentid_[sim->pert.select]; - sim->pert.flexselect = -1; - sim->pert.skinselect = -1; - - // stop perturbation if world reached - if (sim->pert.select<=0) { - sim->pert.active = 0; - } - } - - break; - - case ']': // cycle up fixed cameras - if ((sim->m_ || !sim->is_passive_) && sim->ncam_) { - sim->cam.type = mjCAMERA_FIXED; - // camera = {0 or 1} are reserved for the free and tracking cameras - if (sim->camera < 2 || sim->camera == 2 + sim->ncam_ - 1) { - sim->camera = 2; - } else { - sim->camera += 1; - } - sim->cam.fixedcamid = sim->camera - 2; - mjui0_update_section(sim, SECT_RENDERING); - } - break; - - case '[': // cycle down fixed cameras - if ((sim->m_ || sim->is_passive_) && sim->ncam_) { - sim->cam.type = mjCAMERA_FIXED; - // camera = {0 or 1} are reserved for the free and tracking cameras - if (sim->camera <= 2) { - sim->camera = 2 + sim->ncam_-1; - } else { - sim->camera -= 1; - } - sim->cam.fixedcamid = sim->camera - 2; - mjui0_update_section(sim, SECT_RENDERING); - } - break; - - case mjKEY_F6: // cycle frame visualisation - if (sim->m_ || sim->is_passive_) { - sim->opt.frame = (sim->opt.frame + 1) % mjNFRAME; - mjui0_update_section(sim, SECT_RENDERING); - } - break; - - case mjKEY_F7: // cycle label visualisation - if (sim->m_ || sim->is_passive_) { - sim->opt.label = (sim->opt.label + 1) % mjNLABEL; - mjui0_update_section(sim, SECT_RENDERING); - } - break; - - case mjKEY_ESCAPE: // free camera - sim->cam.type = mjCAMERA_FREE; - sim->camera = 0; - mjui0_update_section(sim, SECT_RENDERING); - break; - - case '-': // slow down - if (!sim->is_passive_) { - int numclicks = sizeof(sim->percentRealTime) / sizeof(sim->percentRealTime[0]); - if (sim->real_time_index < numclicks-1 && !state->shift) { - sim->real_time_index++; - sim->speed_changed = true; - } - } - break; - - case '=': // speed up - if (!sim->is_passive_ && sim->real_time_index > 0 && !state->shift) { - sim->real_time_index--; - sim->speed_changed = true; - } - break; - - case mjKEY_TAB: // toggle left/right UI - if (!state->shift) { - // toggle left UI - sim->ui0_enable = !sim->ui0_enable; - UiModify(&sim->ui0, state, &sim->platform_ui->mjr_context()); - } else { - // toggle right UI - sim->ui1_enable = !sim->ui1_enable; - UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); - } - break; - } - - return; - } - - // local pointers used below - mjModel* model = sim->is_passive_ ? sim->m_passive_ : sim->m_; - mjData* data = sim->is_passive_ ? sim->d_passive_ : sim->d_; - - // 3D scroll - if (state->type==mjEVENT_SCROLL && state->mouserect==3 && model) { - // emulate vertical mouse motion = 2% of window height - mjv_moveCamera(model, mjMOUSE_ZOOM, 0, -zoom_increment*state->sy, &sim->scn, &sim->cam); - return; - } - - // 3D press - if (state->type==mjEVENT_PRESS && state->mouserect==3) { - // set perturbation - int newperturb = 0; - if (state->control && sim->pert.select>0 && (sim->m_ || sim->is_passive_)) { - // right: translate; left: rotate - if (state->right) { - newperturb = mjPERT_TRANSLATE; - } else if (state->left) { - newperturb = mjPERT_ROTATE; - } - if (newperturb && !sim->pert.active) { - sim->pending_.newperturb = newperturb; - } - } - - // handle double-click - if (state->doubleclick && (sim->m_ || sim->is_passive_)) { - sim->pending_.select = true; - std::memcpy(&sim->pending_.select_state, state, sizeof(sim->pending_.select_state)); - - // stop perturbation on select - sim->pert.active = 0; - sim->pending_.newperturb = 0; - } - - return; - } - - // 3D release - if (state->type==mjEVENT_RELEASE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) { - // stop perturbation - sim->pert.active = 0; - sim->pending_.newperturb = 0; - return; - } - - // 3D move - if (state->type==mjEVENT_MOVE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) { - // determine action based on mouse button - mjtMouse action; - if (state->right) { - action = state->shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; - } else if (state->left) { - action = state->shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; - } else { - action = mjMOUSE_ZOOM; - } - - // move perturb or camera - mjrRect r = state->rect[3]; - if (sim->pert.active) { - mjv_movePerturb(model, data, action, state->dx / r.height, -state->dy / r.height, - &sim->scn, &sim->pert); - } else { - mjv_moveCamera(model, action, state->dx / r.height, -state->dy / r.height, - &sim->scn, &sim->cam); - } - return; - } - - // Dropped files - if (state->type == mjEVENT_FILESDROP && state->dropcount > 0 && !sim->is_passive_) { - while (sim->droploadrequest.load()) {} - mju::strcpy_arr(sim->dropfilename, state->droppaths[0]); - sim->droploadrequest.store(true); - return; - } - - // Redraw - if (state->type == mjEVENT_REDRAW) { - sim->Render(); - return; - } -} -} // namespace - -namespace mujoco { -namespace mju = ::mujoco::sample_util; - -Simulate::Simulate(std::unique_ptr platform_ui, - mjvCamera* cam, mjvOption* opt, mjvPerturb* pert, - bool is_passive) - : is_passive_(is_passive), - cam(*cam), - opt(*opt), - pert(*pert), - platform_ui(std::move(platform_ui)), - uistate(this->platform_ui->state()) { - mjv_defaultScene(&scn); -} - - -//------------------------- Synchronize render and physics threads --------------------------------- - -// operations which require holding the mutex, prevents racing with physics thread -void Simulate::Sync(bool state_only) { - MutexLock lock(this->mtx); - - if (!m_) { - return; - } - if (this->exitrequest.load()) { - return; - } - - bool update_profiler = this->profiler; - bool update_sensor = this->sensor; - - for (int i = 0; i < m_->njnt; ++i) { - std::optional> range; - if (m_->jnt_limited[i]) { - range.emplace(m_->jnt_range[2*i], m_->jnt_range[2*i + 1]); - } - if (jnt_range_[i] != range) { - pending_.ui_update_joint = true; - jnt_range_[i].swap(range); - } - } - - for (int i = 0; i < m_->nu; ++i) { - std::optional> range; - if (m_->actuator_ctrllimited[i]) { - range.emplace(m_->actuator_ctrlrange[2*i], m_->actuator_ctrlrange[2*i + 1]); - } - if (actuator_ctrlrange_[i] != range) { - pending_.ui_remake_ctrl = true; - actuator_ctrlrange_[i].swap(range); - } - } - - for (int i = 0; i < m_->nq; ++i) { - if (qpos_[i] != qpos_prev_[i]) { - d_->qpos[i] = qpos_[i]; - } else { - qpos_[i] = d_->qpos[i]; - } - if (qpos_prev_[i] != qpos_[i]) { - pending_.ui_update_joint = true; - qpos_prev_[i] = qpos_[i]; - } - } - - for (int i = 0; i < m_->nu; ++i) { - if (ctrl_[i] != ctrl_prev_[i]) { - d_->ctrl[i] = ctrl_[i]; - } else { - ctrl_[i] = d_->ctrl[i]; - } - if (ctrl_prev_[i] != ctrl_[i]) { - pending_.ui_update_ctrl = true; - ctrl_prev_[i] = ctrl_[i]; - } - } - - for (int i = 0; i < m_->neq; ++i) { - if (eq_active_[i] != eq_active_prev_[i]) { - d_->eq_active[i] = eq_active_[i]; - } else { - eq_active_[i] = d_->eq_active[i]; - } - if (eq_active_prev_[i] != eq_active_[i]) { - pending_.ui_update_equality = true; - eq_active_prev_[i] = eq_active_[i]; - } - } - - // in passive mode, synchronize user's mjModel with changes made via the UI - if (is_passive_) { - // synchronize mjModel.opt - if (std::memcmp(&m_passive_->opt, &mjopt_prev_, sizeof(mjOption))) { - pending_.ui_update_physics = true; - m_->opt = m_passive_->opt; - } - - // synchronize mjModel.vis - if (std::memcmp(&m_passive_->vis, &mjvis_prev_, sizeof(mjVisual))) { - pending_.ui_update_visualization = true; - m_->vis = m_passive_->vis; - } - - // synchronize mjModel.stat - if (std::memcmp(&m_passive_->stat, &mjstat_prev_, sizeof(mjStatistic))) { - pending_.ui_update_visualization = true; - m_->stat = m_passive_->stat; - } - - // synchronize number of mjWARN_VGEOMFULL warnings - if (d_passive_->warning[mjWARN_VGEOMFULL].number > warn_vgeomfull_prev_) { - d_->warning[mjWARN_VGEOMFULL].number += - d_passive_->warning[mjWARN_VGEOMFULL].number - warn_vgeomfull_prev_; - } - } - - if (pending_.save_xml) { - char err[200]; - if (!pending_.save_xml->empty() && !mj_saveLastXML(pending_.save_xml->c_str(), m_, err, 200)) { - std::printf("Save XML error: %s", err); - } - pending_.save_xml = std::nullopt; - } - - if (pending_.save_mjb) { - if (!pending_.save_mjb->empty()) { - mj_saveModel(m_, pending_.save_mjb->c_str(), nullptr, 0); - } - pending_.save_mjb = std::nullopt; - } - - if (pending_.print_model) { - if (!pending_.print_model->empty()) { - mj_printModel(m_, pending_.print_model->c_str()); - } - pending_.print_model = std::nullopt; - } - - if (pending_.print_data) { - if (!pending_.print_data->empty()) { - mj_printData(m_, d_, pending_.print_data->c_str()); - } - pending_.print_data = std::nullopt; - } - - if (pending_.reset) { - mj_resetData(m_, d_); - mj_forward(m_, d_); - load_error[0] = '\0'; - update_profiler = true; - update_sensor = true; - scrub_index = 0; - pending_.ui_update_simulation = true; - pending_.reset = false; - } - - if (pending_.align) { - AlignAndScaleView(this, m_); - pending_.align = false; - } - - if (pending_.copy_key) { - CopyKey(this, m_, d_, pending_.copy_key_full_precision); - pending_.copy_key = false; - pending_.copy_key_full_precision = false; - } - - if (pending_.load_from_history) { - LoadScrubState(this); - update_profiler = true; - update_sensor = true; - pending_.load_from_history = false; - } - - if (pending_.load_key) { - mj_resetDataKeyframe(m_, d_, this->key); - mj_forward(m_, d_); - update_profiler = true; - update_sensor = true; - pending_.load_key = false; - } - - if (pending_.save_key) { - mj_setKeyframe(m_, d_, this->key); - pending_.save_key = false; - } - - if (pending_.zero_ctrl) { - mju_zero(d_->ctrl, m_->nu); - pending_.zero_ctrl = false; - } - - // perturbation onset: reset reference - if (pending_.newperturb) { - mjv_initPerturb(m_, d_, &this->scn, &this->pert); - this->pert.active = pending_.newperturb; - pending_.newperturb = 0; - } - - if (pending_.select) { - // determine selection mode - int selmode; - if (pending_.select_state.button==mjBUTTON_LEFT) { - selmode = 1; - } else if (pending_.select_state.control) { - selmode = 3; - } else { - selmode = 2; - } - - // find geom and 3D click point, get corresponding body - mjrRect r = pending_.select_state.rect[3]; - mjtNum selpnt[3]; - int selgeom, selflex, selskin; - int selbody = mjv_select(m_, d_, &this->opt, - static_cast(r.width) / r.height, - (pending_.select_state.x - r.left) / r.width, - (pending_.select_state.y - r.bottom) / r.height, - &this->scn, selpnt, &selgeom, &selflex, &selskin); - - // set lookat point, start tracking is requested - if (selmode==2 || selmode==3) { - // copy selpnt if anything clicked - if (selbody>=0) { - mju_copy3(this->cam.lookat, selpnt); - } - - // switch to tracking camera if dynamic body clicked - if (selmode==3 && selbody>0) { - // mujoco camera - this->cam.type = mjCAMERA_TRACKING; - this->cam.trackbodyid = selbody; - this->cam.fixedcamid = -1; - - // UI camera - this->camera = 1; - pending_.ui_update_rendering = true; - } - } - - // set body selection - else { - if (selbody>=0) { - // record selection - this->pert.select = selbody; - this->pert.flexselect = selflex; - this->pert.skinselect = selskin; - - // compute localpos - mjtNum tmp[3]; - mju_sub3(tmp, selpnt, d_->xpos + 3*this->pert.select); - mju_mulMatTVec(this->pert.localpos, d_->xmat + 9*this->pert.select, tmp, 3, 3); - } else { - this->pert.select = 0; - this->pert.flexselect = -1; - this->pert.skinselect = -1; - } - } - pending_.select = false; - } - - // update scene or sync data from user in passive mode - if (!is_passive_) { - mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - } else { - if (state_only) { - int state_size = mj_stateSize(m_, mjSTATE_INTEGRATION); - mjtNum* state = new mjtNum[state_size]; - mj_getState(m_, d_, state, mjSTATE_INTEGRATION); - mj_setState(m_passive_, d_passive_, state, mjSTATE_INTEGRATION); - mj_forward(m_passive_, d_passive_); - delete[] state; - } else { - mjv_copyModel(m_passive_, m_); - mjv_copyData(d_passive_, m_passive_, d_); - } - - // append geoms from user_scn to scratch space - if (user_scn) { - user_scn_geoms_.clear(); - user_scn_geoms_.reserve(user_scn->ngeom); - for (int i = 0; i < user_scn->ngeom; ++i) { - user_scn_geoms_.push_back(user_scn->geoms[i]); - } - } - - // 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_ = m_passive_->opt; - mjvis_prev_ = m_passive_->vis; - mjstat_prev_ = m_passive_->stat; - warn_vgeomfull_prev_ = d_passive_->warning[mjWARN_VGEOMFULL].number; - } - - // update settings - UpdateSettings(this, m_); - - // update watch - if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) { - UpdateWatch(this, m_, d_); - } - - // update info text - if (this->info) { - UpdateInfoText(this, m_, d_, this->info_title, this->info_content); - } - if (update_profiler) { UpdateProfiler(this, m_, d_); } - if (update_sensor) { UpdateSensor(this, m_, d_); } - - // clear timers once profiler info has been copied - ClearTimers(d_); - - if (this->run || this->is_passive_) { - // clear old perturbations, apply new - mju_zero(d_->xfrc_applied, 6*m_->nbody); - mjv_applyPerturbPose(m_, d_, &this->pert, 0); // mocap bodies only - mjv_applyPerturbForce(m_, d_, &this->pert); - } else { - mjv_applyPerturbPose(m_, d_, &this->pert, 1); // mocap and dynamic bodies - } -} - -//------------------------- Tell the render thread to load a file and wait ------------------------- -void Simulate::LoadMessage(const char* displayed_filename) { - mju::strcpy_arr(this->filename, displayed_filename); - - { - MutexLock lock(mtx); - this->loadrequest = 3; - } -} - -void Simulate::Load(mjModel* m, mjData* d, const char* displayed_filename) { - this->mnew_ = m; - this->dnew_ = d; - mju::strcpy_arr(this->filename, displayed_filename); - - { - MutexLock lock(mtx); - this->loadrequest = 2; - - // Wait for the render thread to be done loading - // so that we know the old model and data's memory can - // be free'd by the other thread (sometimes python) - cond_loadrequest.wait(lock, [this]() { return this->loadrequest == 0; }); - } -} - -void Simulate::LoadMessageClear(void) { - { - MutexLock lock(mtx); - this->loadrequest = 0; - } -} - - - -//------------------------------------- load mjb or xml model -------------------------------------- -void Simulate::LoadOnRenderThread() { - this->m_ = this->mnew_; - this->d_ = this->dnew_; - - ncam_ = this->m_->ncam; - nkey_ = this->m_->nkey; - body_parentid_.resize(this->m_->nbody); - std::memcpy(body_parentid_.data(), this->m_->body_parentid, - sizeof(this->m_->body_parentid[0]) * this->m_->nbody); - - jnt_type_.resize(this->m_->njnt); - std::memcpy(jnt_type_.data(), this->m_->jnt_type, - sizeof(this->m_->jnt_type[0]) * this->m_->njnt); - - jnt_group_.resize(this->m_->njnt); - std::memcpy(jnt_group_.data(), this->m_->jnt_group, - sizeof(this->m_->jnt_group[0]) * this->m_->njnt); - - jnt_qposadr_.resize(this->m_->njnt); - std::memcpy(jnt_qposadr_.data(), this->m_->jnt_qposadr, - sizeof(this->m_->jnt_qposadr[0]) * this->m_->njnt); - - jnt_range_.clear(); - jnt_range_.reserve(this->m_->njnt); - for (int i = 0; i < this->m_->njnt; ++i) { - if (this->m_->jnt_limited[i]) { - jnt_range_.push_back( - std::make_pair(this->m_->jnt_range[2 * i], this->m_->jnt_range[2 * i + 1])); - } else { - jnt_range_.push_back(std::nullopt); - } - } - - jnt_names_.clear(); - jnt_names_.reserve(this->m_->njnt); - for (int i = 0; i < this->m_->njnt; ++i) { - jnt_names_.emplace_back(this->m_->names + this->m_->name_jntadr[i]); - } - - actuator_group_.resize(this->m_->nu); - std::memcpy(actuator_group_.data(), this->m_->actuator_group, - sizeof(this->m_->actuator_group[0]) * this->m_->nu); - - actuator_ctrlrange_.clear(); - actuator_ctrlrange_.reserve(this->m_->nu); - for (int i = 0; i < this->m_->nu; ++i) { - if (this->m_->actuator_ctrllimited[i]) { - actuator_ctrlrange_.push_back(std::make_pair( - this->m_->actuator_ctrlrange[2 * i], this->m_->actuator_ctrlrange[2 * i + 1])); - } else { - actuator_ctrlrange_.push_back(std::nullopt); - } - } - - actuator_names_.clear(); - actuator_names_.reserve(this->m_->nu); - for (int i = 0; i < this->m_->nu; ++i) { - actuator_names_.emplace_back(this->m_->names + this->m_->name_actuatoradr[i]); - } - - equality_names_.clear(); - equality_names_.reserve(this->m_->neq); - for (int i = 0; i < this->m_->neq; ++i) { - equality_names_.emplace_back(this->m_->names + this->m_->name_eqadr[i]); - } - - qpos_.resize(this->m_->nq); - std::memcpy(qpos_.data(), this->d_->qpos, sizeof(this->d_->qpos[0]) * this->m_->nq); - qpos_prev_ = qpos_; - - ctrl_.resize(this->m_->nu); - std::memcpy(ctrl_.data(), this->d_->ctrl, sizeof(this->d_->ctrl[0]) * this->m_->nu); - ctrl_prev_ = ctrl_; - - eq_active_.resize(this->m_->neq); - std::memcpy(eq_active_.data(), this->d_->eq_active, sizeof(this->d_->eq_active[0]) * this->m_->neq); - eq_active_prev_ = eq_active_; - - // allocate history buffer: smaller of {2000 states, 100 MB} - if (!this->is_passive_) { - 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_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 - history_.clear(); - history_.resize(nhistory_ * state_size_); - history_cursor_ = 0; - scrub_index = 0; - - // fill buffer with initial state - mj_getState(this->m_, this->d_, history_.data(), mjSTATE_INTEGRATION); - for (int i = 1; i < nhistory_; ++i) { - mju_copy(&history_[i * state_size_], history_.data(), state_size_); - } - } - - // re-create scene - mjv_makeScene(this->m_, &this->scn, kMaxGeom); - - this->platform_ui->RefreshMjrContext(this->m_, 50*(this->font+1)); - UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); - UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - - if (!this->platform_ui->IsGPUAccelerated()) { - this->scn.flags[mjRND_SHADOW] = 0; - 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; - this->pert.flexselect = -1; - this->pert.skinselect = -1; - - // align and scale view unless reloading the same file - if (this->filename[0] && - mju::strcmp_arr(this->filename, this->previous_filename)) { - AlignAndScaleView(this, this->m_); - mju::strcpy_arr(this->previous_filename, this->filename); - } - - // update scene in managed mode, in passive mode copy data from user (update in RenderLoop) - if (!is_passive_) { - mjv_updateScene(this->m_, this->d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - } else { - mjopt_prev_ = m_->opt; - opt_prev_ = opt; - cam_prev_ = cam; - warn_vgeomfull_prev_ = d_->warning[mjWARN_VGEOMFULL].number; - - // full copy on init - m_passive_ = mj_copyModel(nullptr, m_); - d_passive_ = mj_copyData(nullptr, m_passive_, d_); - } - - // set window title to model name - if (this->m_->names) { - char title[200] = "MuJoCo : "; - mju::strcat_arr(title, this->m_->names); - platform_ui->SetWindowTitle(title); - } - - // set keyframe range and divisions - this->ui0.sect[SECT_SIMULATION].item[5].slider.range[0] = 0; - this->ui0.sect[SECT_SIMULATION].item[5].slider.range[1] = mjMAX(0, this->m_->nkey - 1); - this->ui0.sect[SECT_SIMULATION].item[5].slider.divisions = mjMAX(1, this->m_->nkey - 1); - - // set scrubber range and divisions - this->ui0.sect[SECT_SIMULATION].item[11].slider.range[0] = 1 - nhistory_; - this->ui0.sect[SECT_SIMULATION].item[11].slider.divisions = nhistory_; - - // rebuild UI sections - MakeUiSections(this, this->m_, this->d_); - - // full ui update - UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); - UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - UpdateSettings(this, this->m_); - - // clear request - this->loadrequest = 0; - cond_loadrequest.notify_all(); - - // set real time index - int numclicks = sizeof(this->percentRealTime) / sizeof(this->percentRealTime[0]); - float min_error = 1e6; - float desired = mju_log(100*this->m_->vis.global.realtime); - for (int click=0; clickpercentRealTime[click]) - desired); - if (error < min_error) { - min_error = error; - this->real_time_index = click; - } - } - - this->mnew_ = nullptr; - this->dnew_ = nullptr; -} - - -//------------------------------------------- rendering -------------------------------------------- - -// render the ui to the window -void Simulate::Render() { - // update rendering context buffer size if required - if (this->platform_ui->EnsureContextSize()) { - UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); - UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - - // get 3D rectangle and reduced for profiler - mjrRect rect = this->uistate.rect[3]; - mjrRect smallrect = rect; - if (this->profiler) { - smallrect.width = rect.width - rect.width/4; - } - - // no model - if (!this->is_passive_ && !this->m_) { - // blank screen - mjr_rectangle(rect, 0.2f, 0.3f, 0.4f, 1); - - // label - if (this->loadrequest) { - mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, "LOADING...", nullptr, - &this->platform_ui->mjr_context()); - } else { - char intro_message[Simulate::kMaxFilenameLength]; - mju::sprintf_arr(intro_message, - "MuJoCo version %s\nDrag-and-drop model file here", mj_versionString()); - mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, intro_message, 0, - &this->platform_ui->mjr_context()); - } - - // show last loading error - if (this->load_error[0]) { - mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->load_error, 0, - &this->platform_ui->mjr_context()); - } - - // render uis - if (this->ui0_enable) { - mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); - } - if (this->ui1_enable) { - mjui_render(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - - // finalize - this->platform_ui->SwapBuffers(); - - return; - } - - // update UI sections from last sync - if (pending_.ui_update_simulation) { - if (this->ui0_enable && this->ui0.sect[SECT_SIMULATION].state) { - mjui0_update_section(this, SECT_SIMULATION); - } - pending_.ui_update_simulation = false; - } - - if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) { - mjui0_update_section(this, SECT_WATCH); - } - - if (pending_.ui_update_physics) { - if (this->ui0_enable && this->ui0.sect[SECT_PHYSICS].state) { - mjui0_update_section(this, SECT_PHYSICS); - } - pending_.ui_update_physics = false; - } - - if (pending_.ui_update_visualization) { - if (this->ui0_enable && this->ui0.sect[SECT_VISUALIZATION].state) { - mjui0_update_section(this, SECT_VISUALIZATION); - } - pending_.ui_update_visualization = false; - } - - if (is_passive_) { - if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state && - (cam_prev_.type != cam.type || - cam_prev_.fixedcamid != cam.fixedcamid || - cam_prev_.trackbodyid != cam.trackbodyid || - opt_prev_.label != opt.label || opt_prev_.frame != opt.frame || - IsDifferent(opt_prev_.flags, opt.flags))) { - pending_.ui_update_rendering = true; - } - - if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state && - (IsDifferent(opt_prev_.geomgroup, opt.geomgroup) || - IsDifferent(opt_prev_.sitegroup, opt.sitegroup) || - IsDifferent(opt_prev_.jointgroup, opt.jointgroup) || - IsDifferent(opt_prev_.tendongroup, opt.tendongroup) || - IsDifferent(opt_prev_.actuatorgroup, opt.actuatorgroup) || - IsDifferent(opt_prev_.flexgroup, opt.flexgroup) || - IsDifferent(opt_prev_.skingroup, opt.skingroup))) { - mjui0_update_section(this, SECT_GROUP); - } - - opt_prev_ = opt; - cam_prev_ = cam; - } - - if (pending_.ui_update_rendering) { - if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state) { - mjui0_update_section(this, SECT_RENDERING); - } - pending_.ui_update_rendering = false; - } - - if (pending_.ui_update_joint) { - if (this->ui1_enable && this->ui1.sect[SECT_JOINT].state) { - mjui_update(SECT_JOINT, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - pending_.ui_update_joint = false; - } - - if (pending_.ui_remake_ctrl) { - if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { - this->ui1.nsect = SECT_CONTROL; - MakeControlSection(this); - this->ui1.nsect = NSECT1; - UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - pending_.ui_remake_ctrl = false; - } - - if (pending_.ui_update_ctrl) { - if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { - mjui_update(SECT_CONTROL, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - pending_.ui_update_ctrl = false; - } - - if (pending_.ui_update_equality) { - if (this->ui1_enable && this->ui1.sect[SECT_EQUALITY].state) { - mjui_update(SECT_EQUALITY, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - pending_.ui_update_equality = false; - } - - // render scene - mjr_render(rect, &this->scn, &this->platform_ui->mjr_context()); - - // show last loading error - if (this->load_error[0]) { - mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->load_error, 0, - &this->platform_ui->mjr_context()); - } - - // show pause/loading label - if (!this->run || this->loadrequest) { - 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()); - } - - // get desired and actual percent-of-real-time - float desiredRealtime = this->percentRealTime[this->real_time_index]; - float actualRealtime = 100 / this->measured_slowdown; - - // if running, check for misalignment of more than 10% - float realtime_offset = mju_abs(actualRealtime - desiredRealtime); - bool misaligned = this->run && realtime_offset > 0.1 * desiredRealtime; - - // make realtime overlay label - char rtlabel[30] = {'\0'}; - if (desiredRealtime != 100.0 || misaligned) { - // print desired realtime - int labelsize = std::snprintf(rtlabel, sizeof(rtlabel), "%g%%", desiredRealtime); - - // if misaligned, append to label - if (misaligned) { - std::snprintf(rtlabel+labelsize, sizeof(rtlabel)-labelsize, " (%-4.1f%%)", actualRealtime); - } - } - - // show real-time overlay - if (rtlabel[0]) { - mjr_overlay(mjFONT_BIG, mjGRID_TOPLEFT, smallrect, rtlabel, nullptr, - &this->platform_ui->mjr_context()); - } - - // show ui 0 - if (this->ui0_enable) { - mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); - } - - // show ui 1 - if (this->ui1_enable) { - mjui_render(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); - } - - // show help - if (this->help) { - mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, help_title, help_content, - &this->platform_ui->mjr_context()); - } - - // show info - if (this->info) { - mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->info_title, this->info_content, - &this->platform_ui->mjr_context()); - } - - // show profiler - if (this->profiler) { - ShowProfiler(this, rect); - } - - // show sensor - if (this->sensor) { - ShowSensor(this, smallrect); - } - - // take screenshot, save to file - if (this->screenshotrequest.exchange(false)) { - const unsigned int h = uistate.rect[0].height; - const unsigned int w = uistate.rect[0].width; - std::unique_ptr rgb(new unsigned char[3*w*h]); - if (!rgb) { - mju_error("could not allocate buffer for screenshot"); - } - mjr_readPixels(rgb.get(), nullptr, uistate.rect[0], &this->platform_ui->mjr_context()); - - // flip up-down - for (int r = 0; r < h/2; ++r) { - unsigned char* top_row = &rgb[3*w*r]; - unsigned char* bottom_row = &rgb[3*w*(h-1-r)]; - std::swap_ranges(top_row, top_row+3*w, bottom_row); - } - - // save as PNG - // TODO(b/241577466): Parse the stem of the filename and use a .PNG extension. - // Unfortunately, if we just yank ".xml"/".mjb" from the filename and append .PNG, the macOS - // 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)) { - mju_error("could not save screenshot"); - } else { - std::printf("saved screenshot: %s\n", path.c_str()); - } - } - } - - // user figures - if (this->newfigurerequest.load() == 1) { - this->user_figures_.clear(); - std::swap(this->user_figures_, this->user_figures_new_); - int value = 1; - this->newfigurerequest.compare_exchange_strong(value, 0); - } - for (auto& [viewport, figure] : this->user_figures_) { - ShowFigure(this, viewport, &figure); - } - - // overlay text - if (this->newtextrequest.load() == 1) { - this->user_texts_.clear(); - std::swap(this->user_texts_, this->user_texts_new_); - int value = 1; - this->newtextrequest.compare_exchange_strong(value, 0); - } - for (auto& [font, gridpos, text1, text2] : this->user_texts_) { - ShowOverlayText(this, rect, font, gridpos, text1, text2); - } - - // user images - if (this->newimagerequest.load() == 1) { - this->user_images_.clear(); - std::swap(this->user_images_, this->user_images_new_); - int value = 1; - this->newimagerequest.compare_exchange_strong(value, 0); - } - for (auto& [viewport, image] : this->user_images_) { - ShowImage(this, viewport, image.get()); - } - - // finalize - this->platform_ui->SwapBuffers(); -} - - - -void Simulate::RenderLoop() { - // Set timer callback (milliseconds) - mjcb_time = Timer; - - // init abstract visualization - mjv_defaultCamera(&this->cam); - mjv_defaultOption(&this->opt); - InitializeProfiler(this); - InitializeSensor(this); - - // make empty scene - if (!is_passive_) { - mjv_defaultScene(&this->scn); - mjv_makeScene(nullptr, &this->scn, kMaxGeom); - } - - if (!this->platform_ui->IsGPUAccelerated()) { - this->scn.flags[mjRND_SHADOW] = 0; - this->scn.flags[mjRND_REFLECTION] = 0; - } - - // select default font - int fontscale = ComputeFontScale(*this->platform_ui); - this->font = fontscale/50 - 1; - - // make empty context - this->platform_ui->RefreshMjrContext(nullptr, fontscale); - - // init state and uis - std::memset(&this->uistate, 0, sizeof(mjuiState)); - std::memset(&this->ui0, 0, sizeof(mjUI)); - std::memset(&this->ui1, 0, sizeof(mjUI)); - - auto [buf_width, buf_height] = this->platform_ui->GetFramebufferSize(); - this->uistate.nrect = 1; - this->uistate.rect[0].width = buf_width; - this->uistate.rect[0].height = buf_height; - - this->ui0.spacing = mjui_themeSpacing(this->spacing); - this->ui0.color = mjui_themeColor(this->color); - this->ui0.predicate = UiPredicate; - this->ui0.rectid = 1; - this->ui0.auxid = 0; - - this->ui1.spacing = mjui_themeSpacing(this->spacing); - this->ui1.color = mjui_themeColor(this->color); - this->ui1.predicate = UiPredicate; - this->ui1.rectid = 2; - this->ui1.auxid = 1; - - // set GUI adapter callbacks - this->uistate.userdata = this; - this->platform_ui->SetEventCallback(UiEvent); - this->platform_ui->SetLayoutCallback(UiLayout); - - // 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()); - - // set VSync to initial value - this->platform_ui->SetVSync(this->vsync); - - frames_ = 0; - last_fps_update_ = mj::Simulate::Clock::now(); - - // run event loop - while (!this->platform_ui->ShouldCloseWindow() && !this->exitrequest.load()) { - { - const MutexLock lock(this->mtx); - - // load model (not on first pass, to show "loading" label) - if (this->loadrequest==1) { - this->LoadOnRenderThread(); - } else if (this->loadrequest == 2) { - this->loadrequest = 1; - } - - // poll and handle events - this->platform_ui->PollEvents(); - - // upload assets if requested - bool upload_notify = false; - if (hfield_upload_ != -1) { - mjr_uploadHField(m_, &platform_ui->mjr_context(), hfield_upload_); - hfield_upload_ = -1; - upload_notify = true; - } - if (mesh_upload_ != -1) { - mjr_uploadMesh(m_, &platform_ui->mjr_context(), mesh_upload_); - mesh_upload_ = -1; - upload_notify = true; - } - if (texture_upload_ != -1) { - mjr_uploadTexture(m_, &platform_ui->mjr_context(), texture_upload_); - texture_upload_ = -1; - upload_notify = true; - } - if (upload_notify) { - cond_upload_.notify_all(); - } - - // update scene, doing a full sync if in fully managed mode - if (!is_passive_) { - Sync(); - } else if (m_passive_ && d_passive_) { - // the user has called Sync() in their code - mjv_updateScene(m_passive_, d_passive_, - &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn); - - // add user geoms to scene - int nusergeom = user_scn_geoms_.size(); - int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom); - if (ngeom < nusergeom) { - mj_warning(d_passive_, mjWARN_VGEOMFULL, this->scn.maxgeom); - } - std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), - ngeom * sizeof(mjvGeom)); - this->scn.ngeom += ngeom; - } - } // MutexLock (unblocks simulation thread) - - // render while simulation is running - this->Render(); - - // update FPS stat, at most 5 times per second - auto now = mj::Simulate::Clock::now(); - double interval = Seconds(now - last_fps_update_).count(); - ++frames_; - if (interval > 0.2) { - last_fps_update_ = now; - fps_ = frames_ / interval; - frames_ = 0; - } - } - - const MutexLock lock(this->mtx); - mjv_freeScene(&this->scn); - if (is_passive_) { - mj_deleteData(d_passive_); - mj_deleteModel(m_passive_); - } - - this->exitrequest.store(2); -} - -// add state to history buffer -void Simulate::AddToHistory() { - if (history_.empty()) { - return; - } - - // circular increment of cursor - history_cursor_ = (history_cursor_ + 1) % nhistory_; - - // add state at cursor - mjtNum* state = &history_[state_size_ * history_cursor_]; - mj_getState(m_, d_, state, mjSTATE_INTEGRATION); -} - -// inject Brownian noise -void Simulate::InjectNoise(int key) { - // 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; inu; 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 - } - - // overwrite midpoint with keyframe, if given - if (key >= 0) { - midpoint = m_->key_ctrl[key*m_->nu+i]; - } - - // 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); - if (!m_ || hfieldid < 0 || hfieldid >= m_->nhfield) { - return; - } - hfield_upload_ = hfieldid; - cond_upload_.wait(lock, [this]() { return hfield_upload_ == -1; }); -} - -void Simulate::UpdateMesh(int meshid) { - MutexLock lock(this->mtx); - if (!m_ || meshid < 0 || meshid >= m_->nmesh) { - return; - } - mesh_upload_ = meshid; - cond_upload_.wait(lock, [this]() { return mesh_upload_ == -1; }); -} - -void Simulate::UpdateTexture(int texid) { - MutexLock lock(this->mtx); - if (!m_ || texid < 0 || texid >= m_->ntex) { - return; - } - texture_upload_ = texid; - cond_upload_.wait(lock, [this]() { return texture_upload_ == -1; }); -} -} // namespace mujoco diff --git a/cmvr-es/simulate/mujoco/mujoco_world/CMakeLists.txt b/cmvr-es/simulate/mujoco/mujoco_world/CMakeLists.txt new file mode 100644 index 00000000..ca69f6ca --- /dev/null +++ b/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) diff --git a/cmvr-es/simulate/mujoco/mujoco_world/include/mujoco_world.h b/cmvr-es/simulate/mujoco/mujoco_world/include/mujoco_world.h new file mode 100644 index 00000000..1ebd4bf9 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h" +#include "devices/abstract_device.h" +#include + +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 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 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& joint_names, + const std::vector& positions); + bool setJointTargetStates(const std::vector& joint_names, + const std::vector& positions, + const std::vector& 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& 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 joints_; + mutable std::string last_error_; + std::thread simulation_thread_; + std::atomic stop_requested_{false}; + std::atomic 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 world() const { return world_; } + static std::shared_ptr worldFor(const std::string& id); + +private: + config::MujocoWorldConfig config_; + std::shared_ptr world_; + + static std::mutex registry_mutex_; + static std::unordered_map> registry_; +}; + +} // namespace cmvr::simulate + +#endif // CMVR_ES_MUJOCO_WORLD_H diff --git a/cmvr-es/simulate/mujoco/mujoco_world/src/mujoco_world.cpp b/cmvr-es/simulate/mujoco/mujoco_world/src/mujoco_world.cpp new file mode 100644 index 00000000..0f29ad02 --- /dev/null +++ b/cmvr-es/simulate/mujoco/mujoco_world/src/mujoco_world.cpp @@ -0,0 +1,776 @@ +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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(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> MujocoWorldDevice::registry_; + +MujocoWorld::MujocoWorld(Options options) +{ + load(options); +} + +MujocoWorld::~MujocoWorld() +{ + stop(); + std::lock_guard lock(mutex_); + clearModelLocked(); +} + +bool MujocoWorld::load(const Options& options) +{ + stop(); + + std::lock_guard 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 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 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 lock(mutex_); + return model_ != nullptr && data_ != nullptr; +} + +bool MujocoWorld::isRunning() const +{ + return running_.load(); +} + +double MujocoWorld::timestep() const +{ + std::lock_guard 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 MujocoWorld::jointNames() const +{ + std::lock_guard lock(mutex_); + std::vector 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 lock(mutex_); + return joints_.find(joint_name) != joints_.end(); +} + +bool MujocoWorld::getJointPosition(const std::string& joint_name, double& position) const +{ + std::lock_guard 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 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 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 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 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 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& joint_names, + const std::vector& positions) +{ + if (joint_names.size() != positions.size()) { + std::lock_guard lock(mutex_); + setLastErrorLocked("joint target batch size mismatch"); + return false; + } + + std::lock_guard lock(mutex_); + std::vector 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& joint_names, + const std::vector& positions, + const std::vector& velocities) +{ + if (joint_names.size() != positions.size() || joint_names.size() != velocities.size()) { + std::lock_guard lock(mutex_); + setLastErrorLocked("joint target state batch size mismatch"); + return false; + } + + std::lock_guard lock(mutex_); + std::vector 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 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& 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::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 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(dt / realtime_factor); + auto next_tick = std::chrono::steady_clock::now() + period; + + while (!stop_requested_.load()) { + { + std::lock_guard 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(); + if (!world_->load(options)) { + CMVR_LOG(ERROR) << "[MujocoWorldDevice] load failed, id=" << id_ + << ", error=" << world_->lastError(); + world_.reset(); + return false; + } + + { + std::lock_guard 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 MujocoWorldDevice::worldFor(const std::string& id) +{ + std::lock_guard lock(registry_mutex_); + const auto it = registry_.find(id); + if (it == registry_.end()) { + return nullptr; + } + return it->second.lock(); +} + +} // namespace cmvr::simulate diff --git a/cmvr-es/test/CMakeLists.txt b/cmvr-es/test/CMakeLists.txt new file mode 100644 index 00000000..7321b7f3 --- /dev/null +++ b/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 +) diff --git a/cmvr-es/test/mujoco_manual_ui_test.cpp b/cmvr-es/test/mujoco_manual_ui_test.cpp new file mode 100644 index 00000000..1d4838b9 --- /dev/null +++ b/cmvr-es/test/mujoco_manual_ui_test.cpp @@ -0,0 +1,237 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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& arm, + const std::string& arm_id, + const std::atomic_bool& stop_requested, + const std::shared_ptr& mujoco_world = nullptr) +{ + if (stop_requested.load()) { + return {}; + } + + std::vector 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::quiet_NaN(); + // } + // std::lock_guard lock(mujoco_world->mutex()); + // const auto* data = mujoco_world->data(); + // return data ? data->time : std::numeric_limits::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(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::quiet_NaN(); + // const double sim_realtime_ratio = + // dt > 1e-6 && std::isfinite(sim_dt) + // ? sim_dt / dt + // : std::numeric_limits::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(arm_id); + auto mujoco_camera = devices.getDevice("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(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(); +} diff --git a/protos/cmvr/config/mujoco_config/mujoco_world_config.proto b/protos/cmvr/config/mujoco_config/mujoco_world_config.proto new file mode 100644 index 00000000..1ae13771 --- /dev/null +++ b/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; +}