Merge branch 'google-deepmind:main' into usd-integration

cmake/MujocoDependencies.cmake

@@ -58,6 +58,11 @@ set(MUJOCO_DEP_VERSION_benchmark
     CACHE STRING "Version of `benchmark` to be fetched."
 )
 
+set(MUJOCO_DEP_VERSION_sdflib
+    492847fa81e46653114da48e8886730ccefed377
+    CACHE STRING "Version of `openVDB` to be fetched."
+)
+
 mark_as_advanced(MUJOCO_DEP_VERSION_lodepng)
 mark_as_advanced(MUJOCO_DEP_VERSION_MarchingCubeCpp)
 mark_as_advanced(MUJOCO_DEP_VERSION_tinyxml2)
@@ -68,6 +73,7 @@ mark_as_advanced(MUJOCO_DEP_VERSION_Eigen3)
 mark_as_advanced(MUJOCO_DEP_VERSION_abseil)
 mark_as_advanced(MUJOCO_DEP_VERSION_gtest)
 mark_as_advanced(MUJOCO_DEP_VERSION_benchmark)
+mark_as_advanced(MUJOCO_DEP_VERSION_sdflib)
 
 include(FetchContent)
 include(FindOrFetch)
@@ -178,6 +184,26 @@ findorfetch(
   EXCLUDE_FROM_ALL
 )
 
+findorfetch(
+  USE_SYSTEM_PACKAGE
+  OFF
+  PACKAGE_NAME
+  sdflib
+  LIBRARY_NAME
+  sdflib
+  GIT_REPO
+  https://github.com/UPC-ViRVIG/SdfLib.git
+  GIT_TAG
+  ${MUJOCO_DEP_VERSION_sdflib}
+  PATCH_COMMAND
+  git apply --reject --whitespace=fix ${CMAKE_SOURCE_DIR}/cmake/sdflib-optional-dependencies.patch
+  TARGETS
+  SdfLib
+  EXCLUDE_FROM_ALL
+)
+target_compile_options(SdfLib PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS})
+target_link_options(SdfLib PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS})
+
 set(ENABLE_DOUBLE_PRECISION ON)
 set(CCD_HIDE_ALL_SYMBOLS ON)
 findorfetch(

cmake/sdflib-optional-dependencies.patch

@@ -0,0 +1,544 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 20551cf..9145032 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -70,14 +70,21 @@ add_custom_target(copyShaders ALL SOURCES ${SHADER_FILES})
+ # Add dependencies
+ add_subdirectory(libs)
+ 
++if(SDFLIB_USE_ENOKI)
++    target_link_libraries(${PROJECT_NAME} PUBLIC enoki)
++    target_link_libraries(${PROJECT_NAME} PUBLIC fcpw)
++    target_compile_definitions(${PROJECT_NAME} PUBLIC -DENOKI_AVAILABLE)
++endif()
++
++if(SDFLIB_USE_ASSIMP)
++    target_link_libraries(${PROJECT_NAME} PUBLIC assimp)
++    target_compile_definitions(${PROJECT_NAME} PUBLIC -DASSIMP_AVAILABLE)
++endif()
++    
+ target_link_libraries(${PROJECT_NAME} PUBLIC glm)
+-target_link_libraries(${PROJECT_NAME} PUBLIC assimp)
+ target_link_libraries(${PROJECT_NAME} PUBLIC args)
+ target_link_libraries(${PROJECT_NAME} PUBLIC spdlog)
+ target_link_libraries(${PROJECT_NAME} PUBLIC cereal)
+-target_link_libraries(${PROJECT_NAME} PUBLIC enoki)
+-target_link_libraries(${PROJECT_NAME} PUBLIC eigen)
+-target_link_libraries(${PROJECT_NAME} PUBLIC fcpw)
+ target_link_libraries(${PROJECT_NAME} PUBLIC stb_image)
+ target_link_libraries(${PROJECT_NAME} PUBLIC icg)
+ 
+@@ -86,17 +93,22 @@ if(CMAKE_CXX_COMPILER_ID MATCHES GNU)
+ endif()
+ 
+ # Add openMP
+-if (CMAKE_CXX_COMPILER_ID STREQUAL "MSVC")
+-    message("Enabling openmp llvm extension")
+-    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp:llvm")
+-else()
+-    find_package(OpenMP)
+-    if(NOT OpenMP_CXX_FOUND)
+-    	message(FATAL_ERROR  "OpenMP not found")
+-    endif()
+-    message("OpenMP version ${OpenMP_CXX_VERSION}")
+-    target_link_libraries(${PROJECT_NAME} PUBLIC OpenMP::OpenMP_CXX)
+-endif()                   
++if(SDFLIB_USE_OPENMP)
++    if (CMAKE_CXX_COMPILER_ID STREQUAL "MSVC")
++        message("Enabling openmp llvm extension")
++        set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp:llvm")
++        target_compile_definitions(${PROJECT_NAME} PUBLIC -DOPENMP_AVAILABLE)
++    else()
++        find_package(OpenMP)
++        if(OpenMP_CXX_FOUND)
++        	message("OpenMP version ${OpenMP_CXX_VERSION}")
++            target_link_libraries(${PROJECT_NAME} PUBLIC OpenMP::OpenMP_CXX)
++            target_compile_definitions(${PROJECT_NAME} PUBLIC -DOPENMP_AVAILABLE)
++        else()
++            message("Disabling openmp")
++        endif()
++    endif()    
++endif()
+ 
+ # Add executable
+ if (NOT UNIX)
+@@ -160,6 +172,7 @@ if(SDFLIB_BUILD_DEBUG_APPS)
+     add_executable(GJKtest src/tools/GJKtest/main.cpp)
+     target_link_libraries(GJKtest PUBLIC ${PROJECT_NAME})
+ 
++    target_link_libraries(${PROJECT_NAME} PUBLIC eigen)
+     add_executable(CalculateInterpolationParameters src/tools/CalculateInterpolationParameters/main.cpp)
+     target_link_libraries(CalculateInterpolationParameters PUBLIC ${PROJECT_NAME})
+ 
+diff --git a/include/SdfLib/ExactOctreeSdf.h b/include/SdfLib/ExactOctreeSdf.h
+index 79ad82d..06dd7ac 100644
+--- a/include/SdfLib/ExactOctreeSdf.h
++++ b/include/SdfLib/ExactOctreeSdf.h
+@@ -214,6 +214,8 @@ private:
+ };
+ }
+ 
++#ifdef OPENMP_AVAILABLE
+ #include "ExactOctreeSdfDepthFirst.h"
++#endif
+ 
+ #endif
+\ No newline at end of file
+diff --git a/include/SdfLib/InterpolationMethods.h b/include/SdfLib/InterpolationMethods.h
+index 077dfb2..f707d5b 100644
+--- a/include/SdfLib/InterpolationMethods.h
++++ b/include/SdfLib/InterpolationMethods.h
+@@ -4,7 +4,10 @@
+ #include <array>
+ 
+ #include "utils/TriangleUtils.h"
++
++#ifdef ENOKI_AVAILABLE
+ #include "enoki/array.h"
++#endif
+ 
+ namespace sdflib
+ {
+@@ -236,15 +239,6 @@ struct TriLinearInterpolation
+ //         outCoeff[63] = 8 * inValues[0][0] + 4 * inValues[0][1] * nodeSize + 4 * inValues[0][2] * nodeSize + 4 * inValues[0][3] * nodeSize + -8 * inValues[1][0] + 4 * inValues[1][1] * nodeSize + -4 * inValues[1][2] * nodeSize + -4 * inValues[1][3] * nodeSize + -8 * inValues[2][0] + -4 * inValues[2][1] * nodeSize + 4 * inValues[2][2] * nodeSize + -4 * inValues[2][3] * nodeSize + 8 * inValues[3][0] + -4 * inValues[3][1] * nodeSize + -4 * inValues[3][2] * nodeSize + 4 * inValues[3][3] * nodeSize + -8 * inValues[4][0] + -4 * inValues[4][1] * nodeSize + -4 * inValues[4][2] * nodeSize + 4 * inValues[4][3] * nodeSize + 8 * inValues[5][0] + -4 * inValues[5][1] * nodeSize + 4 * inValues[5][2] * nodeSize + -4 * inValues[5][3] * nodeSize + 8 * inValues[6][0] + 4 * inValues[6][1] * nodeSize + -4 * inValues[6][2] * nodeSize + -4 * inValues[6][3] * nodeSize + -8 * inValues[7][0] + 4 * inValues[7][1] * nodeSize + 4 * inValues[7][2] * nodeSize + 4 * inValues[7][3] * nodeSize + 0.0f;
+ //     }
+ 
+-//     inline static float interpolateValue(const std::array<float, NUM_COEFFICIENTS>& values, glm::vec3 fracPart) 
+-//     {
+-//         return 0.0f
+-//          + values[0] + values[1] * fracPart[0] + values[2] * fracPart[0] * fracPart[0] + values[3] * fracPart[0] * fracPart[0] * fracPart[0] + values[4] * fracPart[1] + values[5] * fracPart[0] * fracPart[1] + values[6] * fracPart[0] * fracPart[0] * fracPart[1] + values[7] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] + values[8] * fracPart[1] * fracPart[1] + values[9] * fracPart[0] * fracPart[1] * fracPart[1] + values[10] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] + values[11] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] + values[12] * fracPart[1] * fracPart[1] * fracPart[1] + values[13] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] + values[14] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] + values[15] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1]
+-//          + values[16] * fracPart[2] + values[17] * fracPart[0] * fracPart[2] + values[18] * fracPart[0] * fracPart[0] * fracPart[2] + values[19] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[2] + values[20] * fracPart[1] * fracPart[2] + values[21] * fracPart[0] * fracPart[1] * fracPart[2] + values[22] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] + values[23] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] + values[24] * fracPart[1] * fracPart[1] * fracPart[2] + values[25] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] + values[26] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] + values[27] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] + values[28] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] + values[29] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] + values[30] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] + values[31] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2]
+-//          + values[32] * fracPart[2] * fracPart[2] + values[33] * fracPart[0] * fracPart[2] * fracPart[2] + values[34] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] + values[35] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] + values[36] * fracPart[1] * fracPart[2] * fracPart[2] + values[37] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] + values[38] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] + values[39] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] + values[40] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[41] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[42] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[43] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[44] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[45] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[46] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[47] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2]
+-//          + values[48] * fracPart[2] * fracPart[2] * fracPart[2] + values[49] * fracPart[0] * fracPart[2] * fracPart[2] * fracPart[2] + values[50] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] * fracPart[2] + values[51] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] * fracPart[2] + values[52] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[53] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[54] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[55] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[56] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[57] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[58] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[59] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[60] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[61] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[62] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[63] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2];
+-//     }
+-
+ //     inline static void interpolateVertexValues(const std::array<float, NUM_COEFFICIENTS>& values, glm::vec3 fracPart, float nodeSize, std::array<float, VALUES_PER_VERTEX>& outValues)
+ //     {
+ //         outValues[0] = 0.0f
+@@ -383,6 +377,7 @@ struct TriCubicInterpolation
+         outCoeff[63] = 8 * inValues[0][0] + 4 * inValues[0][1] + 4 * inValues[0][2] + 4 * inValues[0][3] + 2 * inValues[0][4] + 2 * inValues[0][5] + 2 * inValues[0][6] + 1 * inValues[0][7] + -8 * inValues[1][0] + 4 * inValues[1][1] + -4 * inValues[1][2] + -4 * inValues[1][3] + 2 * inValues[1][4] + 2 * inValues[1][5] + -2 * inValues[1][6] + 1 * inValues[1][7] + -8 * inValues[2][0] + -4 * inValues[2][1] + 4 * inValues[2][2] + -4 * inValues[2][3] + 2 * inValues[2][4] + -2 * inValues[2][5] + 2 * inValues[2][6] + 1 * inValues[2][7] + 8 * inValues[3][0] + -4 * inValues[3][1] + -4 * inValues[3][2] + 4 * inValues[3][3] + 2 * inValues[3][4] + -2 * inValues[3][5] + -2 * inValues[3][6] + 1 * inValues[3][7] + -8 * inValues[4][0] + -4 * inValues[4][1] + -4 * inValues[4][2] + 4 * inValues[4][3] + -2 * inValues[4][4] + 2 * inValues[4][5] + 2 * inValues[4][6] + 1 * inValues[4][7] + 8 * inValues[5][0] + -4 * inValues[5][1] + 4 * inValues[5][2] + -4 * inValues[5][3] + -2 * inValues[5][4] + 2 * inValues[5][5] + -2 * inValues[5][6] + 1 * inValues[5][7] + 8 * inValues[6][0] + 4 * inValues[6][1] + -4 * inValues[6][2] + -4 * inValues[6][3] + -2 * inValues[6][4] + -2 * inValues[6][5] + 2 * inValues[6][6] + 1 * inValues[6][7] + -8 * inValues[7][0] + 4 * inValues[7][1] + 4 * inValues[7][2] + 4 * inValues[7][3] + -2 * inValues[7][4] + -2 * inValues[7][5] + -2 * inValues[7][6] + 1 * inValues[7][7];
+     }
+ 
++#ifdef ENOKI_AVAILABLE
+     using vec4 = enoki::Array<float, 4>;
+ 
+     inline static float interpolateValue(const std::array<float, NUM_COEFFICIENTS>& values, glm::vec3 fracPart) 
+@@ -433,6 +428,16 @@ struct TriCubicInterpolation
+ 
+         return sum;
+     }
++#else
++    inline static float interpolateValue(const std::array<float, NUM_COEFFICIENTS>& values, glm::vec3 fracPart) 
++    {
++        return 0.0f
++         + values[0] + values[1] * fracPart[0] + values[2] * fracPart[0] * fracPart[0] + values[3] * fracPart[0] * fracPart[0] * fracPart[0] + values[4] * fracPart[1] + values[5] * fracPart[0] * fracPart[1] + values[6] * fracPart[0] * fracPart[0] * fracPart[1] + values[7] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] + values[8] * fracPart[1] * fracPart[1] + values[9] * fracPart[0] * fracPart[1] * fracPart[1] + values[10] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] + values[11] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] + values[12] * fracPart[1] * fracPart[1] * fracPart[1] + values[13] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] + values[14] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] + values[15] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1]
++         + values[16] * fracPart[2] + values[17] * fracPart[0] * fracPart[2] + values[18] * fracPart[0] * fracPart[0] * fracPart[2] + values[19] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[2] + values[20] * fracPart[1] * fracPart[2] + values[21] * fracPart[0] * fracPart[1] * fracPart[2] + values[22] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] + values[23] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] + values[24] * fracPart[1] * fracPart[1] * fracPart[2] + values[25] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] + values[26] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] + values[27] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] + values[28] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] + values[29] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] + values[30] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] + values[31] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2]
++         + values[32] * fracPart[2] * fracPart[2] + values[33] * fracPart[0] * fracPart[2] * fracPart[2] + values[34] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] + values[35] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] + values[36] * fracPart[1] * fracPart[2] * fracPart[2] + values[37] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] + values[38] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] + values[39] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] + values[40] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[41] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[42] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[43] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[44] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[45] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[46] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] + values[47] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2]
++         + values[48] * fracPart[2] * fracPart[2] * fracPart[2] + values[49] * fracPart[0] * fracPart[2] * fracPart[2] * fracPart[2] + values[50] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] * fracPart[2] + values[51] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[2] * fracPart[2] * fracPart[2] + values[52] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[53] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[54] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[55] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[56] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[57] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[58] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[59] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[60] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[61] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[62] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2] + values[63] * fracPart[0] * fracPart[0] * fracPart[0] * fracPart[1] * fracPart[1] * fracPart[1] * fracPart[2] * fracPart[2] * fracPart[2];
++    }
++#endif
+ 
+     inline static glm::vec3 interpolateGradient(const std::array<float, NUM_COEFFICIENTS>& values, glm::vec3 fracPart) 
+     {
+@@ -493,4 +498,4 @@ struct TriCubicInterpolation
+ };
+ }
+ 
+-#endif
+\ No newline at end of file
++#endif
+diff --git a/include/SdfLib/TrianglesInfluence.h b/include/SdfLib/TrianglesInfluence.h
+index 3f3d33f..fc2ca52 100644
+--- a/include/SdfLib/TrianglesInfluence.h
++++ b/include/SdfLib/TrianglesInfluence.h
+@@ -1,7 +1,10 @@
+ #ifndef TRIANGLES_INFLUENCE_H
+ #define TRIANGLES_INFLUENCE_H
+ 
++#ifdef ENOKI_AVAILABLE
+ #include <fcpw/fcpw.h>
++#endif
++
+ #include "utils/Mesh.h"
+ #include "utils/TriangleUtils.h"
+ #include "OctreeSdfUtils.h"
+@@ -1008,6 +1011,7 @@ struct VHQueries
+     }
+ };
+ 
++#ifdef ENOKI_AVAILABLE
+ template<typename T>
+ struct FCPWQueries
+ {
+@@ -1118,6 +1122,8 @@ struct FCPWQueries
+     {
+     }
+ };
++#endif
++
+ }
+ 
+-#endif
+\ No newline at end of file
++#endif
+diff --git a/include/SdfLib/utils/Mesh.h b/include/SdfLib/utils/Mesh.h
+index 28d5486..7d21e44 100644
+--- a/include/SdfLib/utils/Mesh.h
++++ b/include/SdfLib/utils/Mesh.h
+@@ -4,9 +4,11 @@
+ #include <string>
+ #include <vector>
+ #include <glm/glm.hpp>
++#ifdef ASSIMP_AVAILABLE
+ #include <assimp/Importer.hpp>
+ #include <assimp/scene.h>      
+ #include <assimp/postprocess.h>
++#endif
+ #include "SdfLib/utils/UsefullSerializations.h"
+ 
+ namespace sdflib
+@@ -43,6 +45,23 @@ struct BoundingBox
+         return glm::length(glm::max(q,glm::vec3(0.0f))) + glm::min(glm::max(q.x, glm::max(q.y,q.z)),0.0f);
+     }
+ 
++    float getDistance(glm::vec3 point, glm::vec3& outGradient) const
++    {
++        glm::vec3 a = glm::abs(point) - getSize();
++        int k = a[0] > a[1] ? 0 : 1;
++        int l = a[2] > a[k] ? 2 : k;
++        if (a[l] < 0) {
++            outGradient[l] = point[l] / glm::abs(point[l]);
++        } else {
++            glm::vec3 b = glm::max(a, glm::vec3(0.0f));
++            float c = glm::length(b);
++            outGradient[0] = a[0] > 0 ? b[0] / c * point[0] / glm::abs(point[0]) : 0;
++            outGradient[1] = a[1] > 0 ? b[1] / c * point[1] / glm::abs(point[1]) : 0;
++            outGradient[2] = a[2] > 0 ? b[2] / c * point[2] / glm::abs(point[2]) : 0;
++        }
++        return getDistance(point);
++    }
++
+     template<class Archive>
+     void serialize(Archive & archive)
+     {
+@@ -54,8 +73,10 @@ class Mesh
+ {
+ public:
+     Mesh() {}
++#ifdef ASSIMP_AVAILABLE
+     Mesh(std::string filePath);
+     Mesh(const aiMesh* mesh);
++#endif
+     Mesh(glm::vec3* vertices, uint32_t numVertices,
+          uint32_t* indices, uint32_t numIndices);
+ 
+@@ -74,7 +95,9 @@ public:
+     void computeNormals();
+     void applyTransform(glm::mat4 trans);
+ private:
++#ifdef ASSIMP_AVAILABLE
+     void initMesh(const aiMesh* mesh);
++#endif
+ 
+     std::vector<glm::vec3> mVertices;
+     std::vector<uint32_t> mIndices;
+@@ -83,4 +106,4 @@ private:
+ };
+ }
+ 
+-#endif
+\ No newline at end of file
++#endif
+diff --git a/include/SdfLib/utils/TriangleUtils.h b/include/SdfLib/utils/TriangleUtils.h
+index 9f930ed..6ee2304 100644
+--- a/include/SdfLib/utils/TriangleUtils.h
++++ b/include/SdfLib/utils/TriangleUtils.h
+@@ -2,6 +2,7 @@
+ #define TRIANGLE_UTILS_H
+ 
+ #include <glm/glm.hpp>
++#include <algorithm>
+ #include <vector>
+ #include <array>
+ #include <map>
+@@ -401,4 +402,4 @@ namespace TriangleUtils
+ }
+ }
+ 
+-#endif
+\ No newline at end of file
++#endif
+diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt
+index b48bf39..3f143c8 100644
+--- a/libs/CMakeLists.txt
++++ b/libs/CMakeLists.txt
+@@ -14,23 +14,25 @@ if(NOT glm_lib_POPULATED)
+ endif()
+ 
+ # assimp
+-FetchContent_Declare(assimp_lib
+-  GIT_REPOSITORY https://github.com/assimp/assimp.git
+-  GIT_TAG 9519a62dd20799c5493c638d1ef5a6f484e5faf1 # 5.2.5
+-)
+-
+-if(NOT assimp_lib)
+-	FetchContent_Populate(assimp_lib)
+-	
+-	set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
+-	set(BUILD_SHARED_LIBS OFF)
+-	set(ASSIMP_BUILD_ASSIMP_TOOLS OFF)
+-	set(ASSIMP_BUILD_TESTS OFF)
+-	set(ASSIMP_INSTALL OFF)
+-	set(ASSIMP_INJECT_DEBUG_POSTFIX OFF)
+-	set(ASSIMP_BUILD_ASSIMP_VIEW OFF)
++if(SDF_USE_ASSIMP)
++	FetchContent_Declare(assimp_lib
++	  GIT_REPOSITORY https://github.com/assimp/assimp.git
++	  GIT_TAG 9519a62dd20799c5493c638d1ef5a6f484e5faf1 # 5.2.5
++	)
+ 	
+-	add_subdirectory(${assimp_lib_SOURCE_DIR} ${assimp_lib_BINARY_DIR})
++	if(NOT assimp_lib)
++		FetchContent_Populate(assimp_lib)
++		
++		set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
++		set(BUILD_SHARED_LIBS OFF)
++		set(ASSIMP_BUILD_ASSIMP_TOOLS OFF)
++		set(ASSIMP_BUILD_TESTS OFF)
++		set(ASSIMP_INSTALL OFF)
++		set(ASSIMP_INJECT_DEBUG_POSTFIX OFF)
++		set(ASSIMP_BUILD_ASSIMP_VIEW OFF)
++		
++		add_subdirectory(${assimp_lib_SOURCE_DIR} ${assimp_lib_BINARY_DIR})
++	endif()
+ endif()
+ 
+ # args
+@@ -76,43 +78,47 @@ if(NOT cereal_lib_POPULATED)
+ endif()
+ 
+ # Enoki
+-FetchContent_Declare(enoki_lib
+-	GIT_REPOSITORY https://github.com/mitsuba-renderer/enoki.git
+-	GIT_TAG 2a18afa
+-)
+-FetchContent_GetProperties(enoki_lib)
+-if(NOT enoki_lib_POPULATED)
+-	FetchContent_Populate(enoki_lib)
+-	add_library(enoki INTERFACE)
+-	add_subdirectory(${enoki_lib_SOURCE_DIR} ${enoki_lib_BINARY_DIR})
+-	target_include_directories(enoki INTERFACE ${enoki_lib_SOURCE_DIR}/include)
+-endif()
+-
+-# eigen
+-FetchContent_Declare(eigen_lib
+-GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
+-GIT_TAG 46126273552afe13692929523d34006f54c19719 # 3.4
+-)
++if(SDFLIB_USE_ENOKI)
++	FetchContent_Declare(enoki_lib
++		GIT_REPOSITORY https://github.com/mitsuba-renderer/enoki.git
++		GIT_TAG 2a18afa
++	)
++	FetchContent_GetProperties(enoki_lib)
++	if(NOT enoki_lib_POPULATED)
++		FetchContent_Populate(enoki_lib)
++		add_library(enoki INTERFACE)
++		add_subdirectory(${enoki_lib_SOURCE_DIR} ${enoki_lib_BINARY_DIR})
++		target_include_directories(enoki INTERFACE ${enoki_lib_SOURCE_DIR}/include)
++	endif()
+ 
+-FetchContent_GetProperties(eigen_lib)
+-if(NOT eigen_lib_POPULATED)
+-	FetchContent_Populate(eigen_lib)
+-	add_library(eigen INTERFACE)
+-	target_include_directories(eigen INTERFACE ${eigen_lib_SOURCE_DIR})
++ 	# FCPW
++	FetchContent_Declare(fcpw_lib
++		GIT_REPOSITORY https://github.com/rohan-sawhney/fcpw.git
++		GIT_TAG dd65ec2
++	)
++	
++	FetchContent_GetProperties(fcpw_lib)
++	if(NOT fcpw_lib_POPULATED)
++		FetchContent_Populate(fcpw_lib)
++		add_subdirectory(${fcpw_lib_SOURCE_DIR} ${fcpw_lib_BINARY_DIR})
++		target_include_directories(fcpw INTERFACE ${fcpw_lib_SOURCE_DIR})
++	endif()
+ endif()
+ 
+-# FCPW
+-FetchContent_Declare(fcpw_lib
+-	GIT_REPOSITORY https://github.com/rohan-sawhney/fcpw.git
+-	GIT_TAG dd65ec2
+-)
+-
+-FetchContent_GetProperties(fcpw_lib)
+-if(NOT fcpw_lib_POPULATED)
+-	FetchContent_Populate(fcpw_lib)
+-	add_subdirectory(${fcpw_lib_SOURCE_DIR} ${fcpw_lib_BINARY_DIR})
+-	target_include_directories(fcpw INTERFACE ${fcpw_lib_SOURCE_DIR})
+-endif()
++# eigen
++if(SDFLIB_BUILD_DEBUG_APPS)
++	FetchContent_Declare(eigen_lib
++	GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
++	GIT_TAG 46126273552afe13692929523d34006f54c19719 # 3.4
++	)
++	
++	FetchContent_GetProperties(eigen_lib)
++	if(NOT eigen_lib_POPULATED)
++		FetchContent_Populate(eigen_lib)
++		add_library(eigen INTERFACE)
++		target_include_directories(eigen INTERFACE ${eigen_lib_SOURCE_DIR})
++	endif()
++ endif()
+ 
+ # stb
+ add_library(stb_image INTERFACE)
+diff --git a/src/sdf/OctreeSdf.cpp b/src/sdf/OctreeSdf.cpp
+index ef8ed4d..0e1eb97 100644
+--- a/src/sdf/OctreeSdf.cpp
++++ b/src/sdf/OctreeSdf.cpp
+@@ -6,7 +6,9 @@
+ #include "SdfLib/InterpolationMethods.h"
+ #include "sdf/OctreeSdfDepthFirst.h"
+ #include "sdf/OctreeSdfBreadthFirst.h"
++#ifdef OPENMP_AVAILABLE
+ #include "sdf/OctreeSdfBreadthFirstNoDelay.h"
++#endif
+ #include <array>
+ #include <stack>
+ 
+@@ -46,8 +48,11 @@ OctreeSdf::OctreeSdf(const Mesh& mesh, BoundingBox box,
+             break;
+         case OctreeSdf::InitAlgorithm::CONTINUITY:
+             //initOctreeWithContinuity<PerNodeRegionTrianglesInfluence<InterpolationMethod>>(mesh, startDepth, depth, terminationThreshold, terminationRule);
+-            // initOctreeWithContinuity<VHQueries<InterpolationMethod>>(mesh, startDepth, depth, terminationThreshold, terminationRule);
++#ifdef OPENMP_AVAILABLE
+             initOctreeWithContinuityNoDelay<VHQueries<InterpolationMethod>>(mesh, startDepth, depth, terminationThreshold, terminationRule, numThreads);
++#else
++            initOctreeWithContinuity<VHQueries<InterpolationMethod>>(mesh, startDepth, depth, terminationThreshold, terminationRule);
++#endif            
+             break;
+         // case OctreeSdf::InitAlgorithm::GPU_IMPLEMENTATION:
+         //     Timer time;
+@@ -78,7 +83,7 @@ float OctreeSdf::getDistance(glm::vec3 sample) const
+        startArrayPos.y < 0 || startArrayPos.y >= mStartGridSize ||
+        startArrayPos.z < 0 || startArrayPos.z >= mStartGridSize)
+     {
+-        return mBox.getDistance(sample) + glm::sqrt(3.0f) * mBox.getSize().x;
++        return mBox.getDistance(sample) + mMinBorderValue;
+     }
+ 
+     const OctreeNode* currentNode = &mOctreeData[startArrayPos.z * mStartGridXY + startArrayPos.y * mStartGridSize + startArrayPos.x];
+@@ -108,7 +113,7 @@ float OctreeSdf::getDistance(glm::vec3 sample, glm::vec3& outGradient) const
+        startArrayPos.y < 0 || startArrayPos.y >= mStartGridSize ||
+        startArrayPos.z < 0 || startArrayPos.z >= mStartGridSize)
+     {
+-        return mBox.getDistance(sample) + mMinBorderValue;
++        return mBox.getDistance(sample, outGradient) + mMinBorderValue;
+     }
+ 
+     const OctreeNode* currentNode = &mOctreeData[startArrayPos.z * mStartGridXY + startArrayPos.y * mStartGridSize + startArrayPos.x];
+@@ -253,4 +258,4 @@ void OctreeSdf::getDepthDensity(std::vector<float>& depthsDensity)
+         size *= 0.125f;
+     }
+ }
+-}
+\ No newline at end of file
++}
+diff --git a/src/sdf/OctreeSdfDepthFirst.h b/src/sdf/OctreeSdfDepthFirst.h
+index 53ee4b2..196d191 100644
+--- a/src/sdf/OctreeSdfDepthFirst.h
++++ b/src/sdf/OctreeSdfDepthFirst.h
+@@ -7,7 +7,10 @@
+ #include "SdfLib/OctreeSdfUtils.h"
+ #include <array>
+ #include <stack>
++#ifdef OPENMP_AVAILABLE
+ #include <omp.h>
++#endif    
++
+ 
+ namespace sdflib
+ {
+@@ -381,7 +384,9 @@ void OctreeSdf::initOctree(const Mesh& mesh, uint32_t startDepth, uint32_t maxDe
+     };
+ 
+     const uint32_t voxlesPerAxis = 1 << startDepth;
++#ifdef OPENMP_AVAILABLE
+     if(numThreads < 2)
++#endif
+     {
+         // Create the grid
+         mOctreeData.resize(voxlesPerAxis * voxlesPerAxis * voxlesPerAxis);
+@@ -402,7 +407,8 @@ void OctreeSdf::initOctree(const Mesh& mesh, uint32_t startDepth, uint32_t maxDe
+ 
+         mValueRange = mainThread.valueRange;
+     }
+-    else
++#ifdef OPENMP_AVAILABLE
++    else 
+     {
+         std::vector<ThreadContext> threadsContext(numThreads, mainThread);
+ 
+@@ -511,6 +517,7 @@ void OctreeSdf::initOctree(const Mesh& mesh, uint32_t startDepth, uint32_t maxDe
+             }
+         #endif
+     }
++#endif
+ 	
+ #ifdef SDFLIB_PRINT_STATISTICS
+     SPDLOG_INFO("Used an octree of max depth {}", maxDepth);
+@@ -544,4 +551,4 @@ void OctreeSdf::initOctree(const Mesh& mesh, uint32_t startDepth, uint32_t maxDe
+ }
+ }
+ 
+-#endif
+\ No newline at end of file
++#endif
+diff --git a/src/utils/Mesh.cpp b/src/utils/Mesh.cpp
+index b407d38..6fff5fe 100644
+--- a/src/utils/Mesh.cpp
++++ b/src/utils/Mesh.cpp
+@@ -5,6 +5,7 @@
+ 
+ namespace sdflib
+ {
++#ifdef ASSIMP_AVAILABLE
+ Mesh::Mesh(std::string filePath)
+ {
+     Assimp::Importer import;
+@@ -28,6 +29,7 @@ Mesh::Mesh(const aiMesh* mesh)
+ {
+     initMesh(mesh);
+ }
++#endif
+ 
+ Mesh::Mesh(glm::vec3* vertices, uint32_t numVertices,
+          uint32_t* indices, uint32_t numIndices)
+@@ -39,7 +41,7 @@ Mesh::Mesh(glm::vec3* vertices, uint32_t numVertices,
+     std::memcpy(mIndices.data(), indices, sizeof(uint32_t) * numIndices);
+ }
+ 
+-
++#ifdef ASSIMP_AVAILABLE
+ void Mesh::initMesh(const aiMesh* mesh)
+ {
+     if(!(mesh->mPrimitiveTypes & aiPrimitiveType_TRIANGLE))
+@@ -83,6 +85,7 @@ void Mesh::initMesh(const aiMesh* mesh)
+ 		computeNormals();
+ 	}
+ }
++#endif
+ 
+ void Mesh::computeBoundingBox()
+ {
+@@ -134,4 +137,4 @@ void Mesh::applyTransform(glm::mat4 trans)
+ 
+     computeBoundingBox();
+ }
+-}
+\ No newline at end of file
++}

doc/APIreference/APIglobals.rst

@@ -450,9 +450,15 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr
      - The maximal number of real-valued parameters used to define the impedance of each scalar constraint.
        Determines the size of all ``mjModel.XXX_solimp`` fields.
    * - ``mjNSOLVER``
-     - 1000
+     - 200
-     - The size of the preallocated array ``mjData.solver``. This is used to store diagnostic information about each
+     - The number of iterations where solver statistics can be stored in ``mjData.solver``. This array is used
-       iteration of the constraint solver. The actual number of iterations is given by ``mjData.solver_iter``.
+       to store diagnostic information about each iteration of the constraint solver.
+       The actual number of iterations is given by ``mjData.solver_iter``.
+   * - ``mjNISLAND``
+     - 20
+     - The number of islands for which solver statistics can be stored in ``mjData.solver``. This array is
+       used to store diagnostic information about each iteration of the constraint solver.
+       The actual number of islands for which the solver was run is given by ``mjData.nsolver_island``.
    * - ``mjNGROUP``
      - 6
      - The number of geom, site, joint, tendon and actuator groups whose rendering can be enabled and disabled via

doc/APIreference/functions.rst

@@ -3444,7 +3444,7 @@ mju_threadPoolCreate
 
 .. mujoco-include:: mju_threadPoolCreate
 
-Creates a thread pool with the specified number of threads running.
+Create a thread pool with the specified number of threads running.
 
 .. _mju_threadPoolEnqueue:
 
@@ -3453,16 +3453,7 @@ mju_threadPoolEnqueue
 
 .. mujoco-include:: mju_threadPoolEnqueue
 
-Enqueues a task in a thread pool.
+Enqueue a task in a thread pool.
-
-.. _mju_taskJoin:
-
-mju_taskJoin
-~~~~~~~~~~~~
-
-.. mujoco-include:: mju_taskJoin
-
-Waits for a task to complete.
 
 .. _mju_threadPoolDestroy:
 
@@ -3471,5 +3462,23 @@ mju_threadPoolDestroy
 
 .. mujoco-include:: mju_threadPoolDestroy
 
-Destroys a thread pool.
+Destroy a thread pool.
+
+.. _mju_defaultTask:
+
+mju_defaultTask
+~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_defaultTask
+
+Initialize an mjTask.
+
+.. _mju_taskJoin:
+
+mju_taskJoin
+~~~~~~~~~~~~
+
+.. mujoco-include:: mju_taskJoin
+
+Wait for a task to complete.
 

doc/XMLreference.rst

@@ -1322,9 +1322,9 @@ also known as terrain map, is a 2D matrix of elevation data. The data can be spe
 
 .. _asset-hfield-content_type:
 
-:at:`content_type`: :at-val: `string, optional`
+:at:`content_type`: :at-val:`string, optional`
    If the file attribute is specified, then this sets the
-   `Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`_ (formerly known as MIME types) of the
+   `Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`__ (formerly known as MIME types) of the
    file to be loaded. Any filename extensions will be overloaded.  Currently ``image/png`` and
    ``image/vnd.mujoco.hfield`` are supported.
 
@@ -1439,15 +1439,16 @@ Positioning and orienting is complicated by the fact that vertex data are often
 whose origin is not inside the mesh. In contrast, MuJoCo expects the origin of a geom's local frame to coincide with the
 geometric center of the shape. We resolve this discrepancy by pre-processing the mesh in the compiler, so that it is
 centered around (0,0,0) and its principal axes of inertia are the coordinate axes. We also save the translation and
-rotation offsets needed to achieve such alignment. These offsets are then applied to the referencing geom's position and
+rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos<mjModel>` and
-orientation; see also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot
+:ref:`mjModel.mesh_quat<mjModel>`. These offsets are then applied to the referencing geom's position and orientation; see
-models are designed in a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we
+also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in
-set the body frame at the joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh.
+a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we set the body frame at the
-Below is an MJCF model fragment of a forearm, containing all the information needed to put the mesh where one would
+joint, so that the joint position is (0,0,0) in the body frame, and simply reference the mesh. Below is an MJCF model
-expect it to be. The body position is specified relative to the parent body, namely the upper arm (not shown). It is
+fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body
-offset by 35 cm which is the typical length of the human upper arm. If the mesh vertex data were not designed in the
+position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the
-above convention, we would have to use the geom position and orientation (or the new refpos, refquat mechanism) to
+typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have
-compensate, but in practice this is rarely needed.
+to use the geom position and orientation (or the new refpos, refquat mechanism) to compensate, but in practice this is
+rarely needed.
 
 .. code-block:: xml
 
@@ -3054,6 +3055,13 @@ and the +Y axis points up. Thus the frame position and orientation are the key a
    Vertical field of view of the camera, expressed in degrees regardless of the global angle setting. The horizontal
    field of view is computed automatically given the window size and the vertical field of view.
 
+.. _body-camera-resolution:
+
+:at:`resolution`: :at-val:`int(2), "1 1"`
+   Resolution of the camera in pixels [width height]. Note that these values are not used for rendering since those
+   dimensions are determined by the size of the rendering context. This attribute serves as a convenient
+   location to save the required resolution when creating a context.
+
 .. _body-camera-ipd:
 
 :at:`ipd`: :at-val:`real, "0.068"`
@@ -5412,7 +5420,6 @@ site frame. The output is a 3D vector.
 :at:`site`: :at-val:`string, required`
    The site where the sensor is attached.
 
-
 .. _sensor-rangefinder:
 
 :el-prefix:`sensor/` |-| **rangefinder** (*)
@@ -5441,6 +5448,39 @@ excluded; this is because sensor calculations are independent of the visualizer.
 :at:`site`: :at-val:`string, required`
    The site where the sensor is attached.
 
+.. _sensor-camprojection:
+
+:el-prefix:`sensor/` |-| **camprojection** (*)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This element creates a camprojection sensor, which returns the location of a target site, projected onto a camera image
+in pixel coordinates. The origin of this system is located at the top-left corner of the first pixel, so a target
+which projects exactly onto the corner of the image, will have value (0, 0). Values are not clipped, so targets which
+fall outside the camera image will take values above or below the pixel limits. Moreover, points behind the camera
+are also projected onto the image, so it is up to the user to filter out such points, if desired. This can be done using
+a `framepos<sensor-framepos>` sensor with the camera as reference frame, then a negative/positive value in the
+z-coordinate indicates (respectively) a location in the front/back of the camera.
+
+.. _sensor-camprojection-site:
+
+:at:`site`: :at-val:`string, required`
+   The site which is projected on to the camera image.
+
+.. _sensor-camprojection-camera:
+
+:at:`camera`: :at-val:`string, required`
+   The camera used for the projection, its :ref:`resolution<body-camera-resolution>` attribute must be positive.
+
+.. _sensor-camprojection-name:
+
+.. _sensor-camprojection-noise:
+
+.. _sensor-camprojection-cutoff:
+
+.. _sensor-camprojection-user:
+
+:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user`
+   See :ref:`CSensor`.
 
 .. _sensor-jointpos:
 
@@ -6655,6 +6695,8 @@ if omitted.
 
 .. _default-camera-fovy:
 
+.. _default-camera-resolution:
+
 .. _default-camera-ipd:
 
 .. _default-camera-pos:

doc/XMLschema.rst

@@ -364,11 +364,11 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`name<body-camera-name>`                                   | :ref:`class<body-camera-class>`                                 | :ref:`fovy<body-camera-fovy>`                                   | :ref:`ipd<body-camera-ipd>`                                     | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`pos<body-camera-pos>`                                     | :ref:`quat<body-camera-quat>`                                   | :ref:`axisangle<body-camera-axisangle>`                         | :ref:`xyaxes<body-camera-xyaxes>`                               | |
+|                                    |    |    | :ref:`resolution<body-camera-resolution>`                       | :ref:`pos<body-camera-pos>`                                     | :ref:`quat<body-camera-quat>`                                   | :ref:`axisangle<body-camera-axisangle>`                         | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`zaxis<body-camera-zaxis>`                                 | :ref:`euler<body-camera-euler>`                                 | :ref:`mode<body-camera-mode>`                                   | :ref:`target<body-camera-target>`                               | |
+|                                    |    |    | :ref:`xyaxes<body-camera-xyaxes>`                               | :ref:`zaxis<body-camera-zaxis>`                                 | :ref:`euler<body-camera-euler>`                                 | :ref:`mode<body-camera-mode>`                                   | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`user<body-camera-user>`                                   |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`target<body-camera-target>`                               | :ref:`user<body-camera-user>`                                   |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| body |br| |_| |L|              |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -835,6 +835,15 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
+| :ref:`camprojection                | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
+| <sensor-camprojection>`            |    |                                                                                                                                                                                                                                                                              |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`name<sensor-camprojection-name>`                          | :ref:`site<sensor-camprojection-site>`                          | :ref:`camera<sensor-camprojection-camera>`                      | :ref:`cutoff<sensor-camprojection-cutoff>`                      | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`noise<sensor-camprojection-noise>`                        | :ref:`user<sensor-camprojection-user>`                          |                                                                 |                                                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| |_| sensor |br| |_| |L|            |    | .. table::                                                                                                                                                                                                                                                                   |
 | :ref:`rangefinder                  | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |
 | <sensor-rangefinder>`              |    |                                                                                                                                                                                                                                                                              |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
@@ -1223,9 +1232,9 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`fovy<default-camera-fovy>`                                | :ref:`ipd<default-camera-ipd>`                                  | :ref:`pos<default-camera-pos>`                                  | :ref:`quat<default-camera-quat>`                                | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`axisangle<default-camera-axisangle>`                      | :ref:`xyaxes<default-camera-xyaxes>`                            | :ref:`zaxis<default-camera-zaxis>`                              | :ref:`euler<default-camera-euler>`                              | |
+|                                    |    |    | :ref:`resolution<default-camera-resolution>`                    | :ref:`axisangle<default-camera-axisangle>`                      | :ref:`xyaxes<default-camera-xyaxes>`                            | :ref:`zaxis<default-camera-zaxis>`                              | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`mode<default-camera-mode>`                                | :ref:`user<default-camera-user>`                                |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`euler<default-camera-euler>`                              | :ref:`mode<default-camera-mode>`                                | :ref:`user<default-camera-user>`                                |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| default |br| |_| |L|           |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -51,30 +51,45 @@ General
    6. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``,
       and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s.
 
-7. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
+   7. Changed :ref:`mjData.solver<mjData>`, the array used to collect solver diagnostic information.
+      This array of :ref:`mjSolverStat` structs is now of length ``mjNISLAND * mjNSOLVER``, interpreted as as a matrix.
+      Each row of length ``mjNSOLVER`` contains separate solver statistics for each constraint island.
+      If the solver does not use islands, only row 0 is filled.
+
+      - The new constant :ref:`mjNISLAND<glNumeric>` was set to 20.
+      - :ref:`mjNSOLVER<glNumeric>` was reduced from 1000 to 200.
+      - Added :ref:`mjData.solver_nisland<mjData>`: the number of islands for which the solver ran.
+      - Renamed ``mjData.solver_iter`` to ``solver_niter``. Both this member and ``mjData.solver_nnz`` are now integer
+        vectors of length ``mjNISLAND``.
+
+8. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
    the exact formula rather than with Euler integration.
-8. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
+9. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
    actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
-9. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
+10. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
-   :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
+    :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
-   :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
+    :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
-10. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
+11. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
     Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
-11. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
+12. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
     :ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
-12. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
+13. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
     linesearch stopping criteria in CG and Newton solvers. This can be useful for performance tuning.
+14. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store normalizing transformation.
+15. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
+    sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a
+    target site, projected onto the camera image, in pixel coordinates.
 
 Python bindings
 ^^^^^^^^^^^^^^^
 
-13. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
+16. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
     camera name used the default camera.
 
 Bug fixes
 ^^^^^^^^^
 
-14. Fixed a bug that was causing the geom margins to be ignored during the midphase.
+17. Fixed a bug that was causing the geom margins to be ignored during the midphase.
 
 
 Version 2.3.7 (July 20, 2023)
@@ -90,8 +105,8 @@ General
    :ref:`Cartesian actuator<actuator-general-refsite>` forces are realizable by individual motors at the joints.
    See :ref:`CForceRange` for details.
 #. Added an optional ``content_type`` attribute to hfield, texture, and mesh assets. This attribute supports a formatted
-   `Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`_ (previously known as MIME type) string
+   `Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`__ (previously known as MIME type)
-   used to determine the type of the asset file without resorting to pulling the type from the file extension.
+   string used to determine the type of the asset file without resorting to pulling the type from the file extension.
 #. Added analytic derivatives for quaternion :ref:`subtraction<mjd_subQuat>` and :ref:`integration<mjd_quatIntegrate>`
    (rotation with an angular velocity). Derivatives are in the 3D tangent space.
 #. Added :ref:`mjv_connector` which has identical functionality to :ref:`mjv_makeConnector`, but with more convenient

doc/includes/references.h

@@ -146,9 +146,10 @@ struct mjData_ {
   mjTimerStat   timer[mjNTIMER];      // timer statistics
 
   // solver statistics
-  mjSolverStat  solver[mjNSOLVER];  // solver statistics per iteration
+  mjSolverStat  solver[mjNISLAND*mjNSOLVER];  // solver statistics per island, per iteration
-  int     solver_iter;              // number of solver iterations
+  int     solver_nisland;           // number of islands processed by solver
-  int     solver_nnz;               // number of non-zeros in Hessian or efc_AR
+  int     solver_niter[mjNISLAND];  // number of solver iterations, per island
+  int     solver_nnz[mjNISLAND];    // number of non-zeros in Hessian or efc_AR, per island
   mjtNum  solver_fwdinv[2];         // forward-inverse comparison: qfrc, efc
 
   // collision statistics
@@ -577,6 +578,7 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_TORQUE,                  // 3D torque between site's body and its parent body
   mjSENS_MAGNETOMETER,            // 3D magnetometer
   mjSENS_RANGEFINDER,             // scalar distance to nearest geom or site along z-axis
+  mjSENS_CAMPROJECTION,           // pixel coordinates of a site in the camera image
 
   // sensors related to scalar joints, tendons, actuators
   mjSENS_JOINTPOS,                // scalar joint position (hinge and slide only)
@@ -1010,6 +1012,7 @@ struct mjModel_ {
   mjtNum*   cam_poscom0;          // global position rel. to sub-com in qpos0 (ncam x 3)
   mjtNum*   cam_pos0;             // global position rel. to body in qpos0    (ncam x 3)
   mjtNum*   cam_mat0;             // global orientation in qpos0              (ncam x 9)
+  int*      cam_resolution;       // [width, height] in pixels                (ncam x 2)
   mjtNum*   cam_fovy;             // y-field of view (deg)                    (ncam x 1)
   mjtNum*   cam_ipd;              // inter-pupilary distance                  (ncam x 1)
   mjtNum*   cam_user;             // user data                                (ncam x nuser_cam)
@@ -1045,6 +1048,8 @@ struct mjModel_ {
   int*      mesh_texcoordadr;     // texcoord data address; -1: no texcoord   (nmesh x 1)
   int*      mesh_texcoordnum;     // number of texcoord                       (nmesh x 1)
   int*      mesh_graphadr;        // graph data address; -1: no graph         (nmesh x 1)
+  mjtNum*   mesh_pos;             // translation applied to asset vertices    (nmesh x 3)
+  mjtNum*   mesh_quat;            // rotation applied to asset vertices       (nmesh x 4)
   float*    mesh_vert;            // vertex positions for all meshes          (nmeshvert x 3)
   float*    mesh_normal;          // normals for all meshes                   (nmeshnormal x 3)
   float*    mesh_texcoord;        // vertex texcoords for all meshes          (nmeshtexcoord x 2)
@@ -1441,14 +1446,21 @@ struct mjrContext_ {              // custom OpenGL context
   int     readPixelFormat;        // default color pixel format for mjr_readPixels
 };
 typedef struct mjrContext_ mjrContext;
-struct mjTask_ {
+typedef enum mjtTaskStatus_ {  // status values for mjTask
-  char buffer[24];
+  mjTASK_NEW = 0,              // newly created
-};
+  mjTASK_QUEUED,               // enqueued in a thread pool
-typedef struct mjTask_ mjTask;
+  mjTASK_COMPLETED             // completed execution
+} mjtTaskStatus;
 struct mjThreadPool_ {
-  char buffer[6208];
+  int nworker;  // number of workers in the pool
 };
 typedef struct mjThreadPool_ mjThreadPool;
+struct mjTask_ {        // a task that can be executed by a thread pool.
+  mjfTask func;         // pointer to the function that implements the task
+  void* args;           // arguments to func
+  volatile int status;  // status of the task
+};
+typedef struct mjTask_ mjTask;
 typedef enum mjtButton_ {         // mouse button
   mjBUTTON_NONE = 0,              // no button
   mjBUTTON_LEFT,                  // left button
@@ -2586,9 +2598,8 @@ int mjp_resourceProviderCount(void);
 const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name);
 const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
 mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
-void mju_threadPoolEnqueue(
+void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
-ThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
-id* args);
-void mju_taskJoin(mjTask* task);
 void mju_threadPoolDestroy(mjThreadPool* thread_pool);
+void mju_defaultTask(mjTask* task);
+void mju_taskJoin(mjTask* task);
 // NOLINTEND

doc/models.rst

@@ -12,8 +12,8 @@ by Google DeepMind.
 
 For more information, visit the `Menagerie repository <https://github.com/google-deepmind/mujoco_menagerie>`__.
 
-Bipeds & Quadrupeds
+Bipeds
-^^^^^^^^^^^^^^^^^^^
+^^^^^^
 
 .. list-table::
    :header-rows: 1
@@ -22,12 +22,38 @@ Bipeds & Quadrupeds
      - Preview
    * - `Agility Cassie <https://github.com/google-deepmind/mujoco_menagerie/tree/main/agility_cassie>`_
      - .. youtube:: rcdsAdwNhtc
+   * - `Robotis OP3 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/robotis_op3>`_
+     - .. youtube:: jLZ3sdkyz_w
+
+Mobile Manipulators
+^^^^^^^^^^^^^^^^^^^
+
+.. list-table::
+   :header-rows: 1
+
+   * - Model
+     - Preview
+   * - `Hello Robot Stretch 2 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/hello_robot_stretch>`_
+     - .. youtube:: w_NUKO61wIc
+
+Quadrupeds
+^^^^^^^^^^
+
+.. list-table::
+   :header-rows: 1
+
+   * - Model
+     - Preview
    * - `Unitree A1 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_a1>`_
      - .. youtube:: paQMrMtnTtc
+   * - `Unitree Go1 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/unitree_go1>`_
+     - .. youtube:: 4d7I67BzDJg
    * - `Anybotics ANYmal B <https://github.com/google-deepmind/mujoco_menagerie/tree/main/anybotics_anymal_b>`_
      - .. youtube:: fRHau-PMGgM
    * - `Anybotics ANYmal C <https://github.com/google-deepmind/mujoco_menagerie/tree/main/anybotics_anymal_c>`_
      - .. youtube:: v04uJWBLwFQ
+   * - `Google Barkour v0 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/google_barkour_v0>`_
+     - .. youtube:: w9EA0joEAMo
 
 Grippers & Hands
 ^^^^^^^^^^^^^^^^
@@ -41,6 +67,8 @@ Grippers & Hands
      - .. youtube:: wi_zJzRm8Ic
    * - `Robotiq 2F-85 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/robotiq_2f85>`_
      - .. youtube:: yYm9fLj32Xw
+   * - `Wonik Allegro <https://github.com/google-deepmind/mujoco_menagerie/tree/main/wonik_allegro>`_
+     - .. youtube:: jDWko1WTRXc
 
 Arms
 ^^^^
@@ -54,4 +82,20 @@ Arms
      - .. youtube:: H5zSrWcJlGs
    * - `Universal Robots UR5e <https://github.com/google-deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e>`_
      - .. youtube:: gAqwNeY0juo
+   * - `LBR iiwa14 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/kuka_iiwa_14>`_
+     - .. youtube:: 4Z44nkNXkwo
+   * - `UFACTORY xArm7 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/ufactory_xarm7>`_
+     - .. youtube:: mMDisja5ark
+   * - `Rethink Robotics Sawyer <https://github.com/google-deepmind/mujoco_menagerie/tree/main/rethink_robotics_sawyer>`_
+     - .. youtube:: sZ41oklVvBg
 
+Drones
+^^^^^^
+
+.. list-table::
+   :header-rows: 1
+
+   * - Model
+     - Preview
+   * - `Skydio X2 <https://github.com/google-deepmind/mujoco_menagerie/tree/main/skydio_x2>`_
+     - .. youtube:: LBsvsgnSvoM

doc/programming/samples.rst

@@ -118,7 +118,7 @@ Windows power plan so that the minimum processor state is 100%.
 .. _saRecord:
 
 `record <https://github.com/google-deepmind/mujoco/blob/main/sample/record.cc>`_
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 This code sample simulates the passive dynamics of a given model, renders it offscreen, reads the color and depth pixel
 values, and saves them into a raw data file that can then be converted into a movie file with tools such as ffmpeg. The

include/mujoco/mjdata.h

@@ -173,9 +173,10 @@ struct mjData_ {
   mjTimerStat   timer[mjNTIMER];      // timer statistics
 
   // solver statistics
-  mjSolverStat  solver[mjNSOLVER];  // solver statistics per iteration
+  mjSolverStat  solver[mjNISLAND*mjNSOLVER];  // solver statistics per island, per iteration
-  int     solver_iter;              // number of solver iterations
+  int     solver_nisland;           // number of islands processed by solver
-  int     solver_nnz;               // number of non-zeros in Hessian or efc_AR
+  int     solver_niter[mjNISLAND];  // number of solver iterations, per island
+  int     solver_nnz[mjNISLAND];    // number of non-zeros in Hessian or efc_AR, per island
   mjtNum  solver_fwdinv[2];         // forward-inverse comparison: qfrc, efc
 
   // collision statistics

include/mujoco/mjmodel.h

@@ -41,7 +41,8 @@
 #define mjNFLUID        12        // number of fluid interaction parameters
 #define mjNREF          2         // number of solver reference parameters
 #define mjNIMP          5         // number of solver impedance parameters
-#define mjNSOLVER       1000      // size of mjData.solver_XXX arrays
+#define mjNSOLVER       200       // size of one mjData.solver array
+#define mjNISLAND       20        // number of mjData.solver arrays
 
 
 //---------------------------------- enum types (mjt) ----------------------------------------------
@@ -283,6 +284,7 @@ typedef enum mjtSensor_ {         // type of sensor
   mjSENS_TORQUE,                  // 3D torque between site's body and its parent body
   mjSENS_MAGNETOMETER,            // 3D magnetometer
   mjSENS_RANGEFINDER,             // scalar distance to nearest geom or site along z-axis
+  mjSENS_CAMPROJECTION,           // pixel coordinates of a site in the camera image
 
   // sensors related to scalar joints, tendons, actuators
   mjSENS_JOINTPOS,                // scalar joint position (hinge and slide only)
@@ -745,6 +747,7 @@ struct mjModel_ {
   mjtNum*   cam_poscom0;          // global position rel. to sub-com in qpos0 (ncam x 3)
   mjtNum*   cam_pos0;             // global position rel. to body in qpos0    (ncam x 3)
   mjtNum*   cam_mat0;             // global orientation in qpos0              (ncam x 9)
+  int*      cam_resolution;       // [width, height] in pixels                (ncam x 2)
   mjtNum*   cam_fovy;             // y-field of view (deg)                    (ncam x 1)
   mjtNum*   cam_ipd;              // inter-pupilary distance                  (ncam x 1)
   mjtNum*   cam_user;             // user data                                (ncam x nuser_cam)
@@ -780,6 +783,8 @@ struct mjModel_ {
   int*      mesh_texcoordadr;     // texcoord data address; -1: no texcoord   (nmesh x 1)
   int*      mesh_texcoordnum;     // number of texcoord                       (nmesh x 1)
   int*      mesh_graphadr;        // graph data address; -1: no graph         (nmesh x 1)
+  mjtNum*   mesh_pos;             // translation applied to asset vertices    (nmesh x 3)
+  mjtNum*   mesh_quat;            // rotation applied to asset vertices       (nmesh x 4)
   float*    mesh_vert;            // vertex positions for all meshes          (nmeshvert x 3)
   float*    mesh_normal;          // normals for all meshes                   (nmeshnormal x 3)
   float*    mesh_texcoord;        // vertex texcoords for all meshes          (nmeshtexcoord x 2)

include/mujoco/mjthread.h

@@ -15,33 +15,26 @@
 #ifndef MUJOCO_INCLUDE_MJTHREAD_H_
 #define MUJOCO_INCLUDE_MJTHREAD_H_
 
-// C API for MuJoCo threading
+typedef enum mjtTaskStatus_ {  // status values for mjTask
-#ifdef __cplusplus
+  mjTASK_NEW = 0,              // newly created
-extern "C" {
+  mjTASK_QUEUED,               // enqueued in a thread pool
-#endif
+  mjTASK_COMPLETED             // completed execution
+} mjtTaskStatus;
 
-#include <stddef.h>
+// function pointer type for mjTask
-
+typedef void* (*mjfTask)(void*);
-#include <mujoco/mjexport.h>
-
-// These types are implemented in C++, they're just used as opaque pointers in C
-// to provide type safety for functions.
-struct mjTask_ {
-  char buffer[24];
-};
-typedef struct mjTask_ mjTask;
 
+// An opaque type representing a thread pool.
 struct mjThreadPool_ {
-  char buffer[6208];
+  int nworker;  // number of workers in the pool
 };
 typedef struct mjThreadPool_ mjThreadPool;
 
-typedef void*(*mjStartRoutine_)(void*);
+struct mjTask_ {        // a task that can be executed by a thread pool.
-typedef mjStartRoutine_ mjStartRoutine;
+  mjfTask func;         // pointer to the function that implements the task
-
+  void* args;           // arguments to func
-#ifdef __cplusplus
+  volatile int status;  // status of the task
-}
+};
-#endif
+typedef struct mjTask_ mjTask;
-
 
 #endif  // MUJOCO_INCLUDE_MJTHREAD_H_

include/mujoco/mjxmacro.h

@@ -258,6 +258,7 @@
     X   ( int,     cam_mode,              ncam,          1                    ) \
     X   ( int,     cam_bodyid,            ncam,          1                    ) \
     X   ( int,     cam_targetbodyid,      ncam,          1                    ) \
+    X   ( int,     cam_resolution,        ncam,          2                    ) \
     X   ( mjtNum,  cam_pos,               ncam,          3                    ) \
     X   ( mjtNum,  cam_quat,              ncam,          4                    ) \
     X   ( mjtNum,  cam_poscom0,           ncam,          3                    ) \
@@ -294,6 +295,8 @@
     XMJV( int,     mesh_bvhadr,           nmesh,         1                    ) \
     XMJV( int,     mesh_bvhnum,           nmesh,         1                    ) \
     XMJV( int,     mesh_graphadr,         nmesh,         1                    ) \
+    X   ( mjtNum,  mesh_pos,              nmesh,         3                    ) \
+    X   ( mjtNum,  mesh_quat,             nmesh,         4                    ) \
     X   ( float,   mesh_vert,             nmeshvert,     3                    ) \
     X   ( float,   mesh_normal,           nmeshnormal,   3                    ) \
     X   ( float,   mesh_texcoord,         nmeshtexcoord, 2                    ) \
@@ -633,8 +636,7 @@
     X( size_t,    maxuse_arena       ) \
     X( int,       maxuse_con         ) \
     X( int,       maxuse_efc         ) \
-    X( int,       solver_iter        ) \
+    X( int,       solver_nisland     ) \
-    X( int,       solver_nnz         ) \
     X( int,       nbodypair_broad    ) \
     X( int,       nbodypair_narrow   ) \
     X( int,       ngeompair_mid      ) \
@@ -651,12 +653,14 @@
 
 
 // vector fields of mjData
-#define MJDATA_VECTOR                                   \
+#define MJDATA_VECTOR                                              \
-    X( mjWarningStat,  warning,        mjNWARNING,  1 ) \
+    X( mjWarningStat,  warning,           mjNWARNING,  1         ) \
-    X( mjTimerStat,    timer,          mjNTIMER,    1 ) \
+    X( mjTimerStat,    timer,             mjNTIMER,    1         ) \
-    X( mjSolverStat,   solver,         mjNSOLVER,   1 ) \
+    X( mjSolverStat,   solver,            mjNILSAND,   mjNSOLVER ) \
-    X( mjtNum,         solver_fwdinv,  2,           1 ) \
+    X( int,            solver_niter,      mjNISLAND,   1         ) \
-    X( mjtNum,         energy,         2,           1 )
+    X( int,            solver_nnz,        mjNISLAND,   1         ) \
+    X( mjtNum,         solver_fwdinv,     2,           1         ) \
+    X( mjtNum,         energy,            2,           1         )
 
 
 // alias XMJV to be the same as X

include/mujoco/mujoco.h

@@ -1307,19 +1307,21 @@ MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
 
 //---------------------- Thread -------------------------------------------------------------------
 
-// Creates a thread pool with the specified number of threads running.
+// Create a thread pool with the specified number of threads running.
 MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
 
-// Enqueues a task in a thread pool.
+// Enqueue a task in a thread pool.
-MJAPI void mju_threadPoolEnqueue(
+MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
-    mjThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
-    void* args);
 
-// Waits for a task to complete.
+// Destroy a thread pool.
+MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool);
+
+// Initialize an mjTask.
+MJAPI void mju_defaultTask(mjTask* task);
+
+// Wait for a task to complete.
 MJAPI void mju_taskJoin(mjTask* task);
 
-// Destroys a thread pool.
-MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool);
 
 #if defined(__cplusplus)
 }

introspect/codegen/generate_enums.py

@@ -56,7 +56,10 @@ class MjEnumVisitor:
       child_kind = child.get('kind')
       if child_kind == 'EnumConstantDecl':
         next_idx = values[-1][1] + 1 if values else 0
-        value = int(child['inner'][0].get('value', next_idx))
+        if 'inner' in child:
+          value = int(child['inner'][0].get('value', next_idx))
+        else:
+          value = next_idx
         values.append((child['name'], value))
     return ast_nodes.EnumDecl(name=name, declname=name, values=dict(values))
 

introspect/enums.py

@@ -313,37 +313,38 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjSENS_TORQUE', 5),
              ('mjSENS_MAGNETOMETER', 6),
              ('mjSENS_RANGEFINDER', 7),
-             ('mjSENS_JOINTPOS', 8),
+             ('mjSENS_CAMPROJECTION', 8),
-             ('mjSENS_JOINTVEL', 9),
+             ('mjSENS_JOINTPOS', 9),
-             ('mjSENS_TENDONPOS', 10),
+             ('mjSENS_JOINTVEL', 10),
-             ('mjSENS_TENDONVEL', 11),
+             ('mjSENS_TENDONPOS', 11),
-             ('mjSENS_ACTUATORPOS', 12),
+             ('mjSENS_TENDONVEL', 12),
-             ('mjSENS_ACTUATORVEL', 13),
+             ('mjSENS_ACTUATORPOS', 13),
-             ('mjSENS_ACTUATORFRC', 14),
+             ('mjSENS_ACTUATORVEL', 14),
-             ('mjSENS_JOINTACTFRC', 15),
+             ('mjSENS_ACTUATORFRC', 15),
-             ('mjSENS_BALLQUAT', 16),
+             ('mjSENS_JOINTACTFRC', 16),
-             ('mjSENS_BALLANGVEL', 17),
+             ('mjSENS_BALLQUAT', 17),
-             ('mjSENS_JOINTLIMITPOS', 18),
+             ('mjSENS_BALLANGVEL', 18),
-             ('mjSENS_JOINTLIMITVEL', 19),
+             ('mjSENS_JOINTLIMITPOS', 19),
-             ('mjSENS_JOINTLIMITFRC', 20),
+             ('mjSENS_JOINTLIMITVEL', 20),
-             ('mjSENS_TENDONLIMITPOS', 21),
+             ('mjSENS_JOINTLIMITFRC', 21),
-             ('mjSENS_TENDONLIMITVEL', 22),
+             ('mjSENS_TENDONLIMITPOS', 22),
-             ('mjSENS_TENDONLIMITFRC', 23),
+             ('mjSENS_TENDONLIMITVEL', 23),
-             ('mjSENS_FRAMEPOS', 24),
+             ('mjSENS_TENDONLIMITFRC', 24),
-             ('mjSENS_FRAMEQUAT', 25),
+             ('mjSENS_FRAMEPOS', 25),
-             ('mjSENS_FRAMEXAXIS', 26),
+             ('mjSENS_FRAMEQUAT', 26),
-             ('mjSENS_FRAMEYAXIS', 27),
+             ('mjSENS_FRAMEXAXIS', 27),
-             ('mjSENS_FRAMEZAXIS', 28),
+             ('mjSENS_FRAMEYAXIS', 28),
-             ('mjSENS_FRAMELINVEL', 29),
+             ('mjSENS_FRAMEZAXIS', 29),
-             ('mjSENS_FRAMEANGVEL', 30),
+             ('mjSENS_FRAMELINVEL', 30),
-             ('mjSENS_FRAMELINACC', 31),
+             ('mjSENS_FRAMEANGVEL', 31),
-             ('mjSENS_FRAMEANGACC', 32),
+             ('mjSENS_FRAMELINACC', 32),
-             ('mjSENS_SUBTREECOM', 33),
+             ('mjSENS_FRAMEANGACC', 33),
-             ('mjSENS_SUBTREELINVEL', 34),
+             ('mjSENS_SUBTREECOM', 34),
-             ('mjSENS_SUBTREEANGMOM', 35),
+             ('mjSENS_SUBTREELINVEL', 35),
-             ('mjSENS_CLOCK', 36),
+             ('mjSENS_SUBTREEANGMOM', 36),
-             ('mjSENS_PLUGIN', 37),
+             ('mjSENS_CLOCK', 37),
-             ('mjSENS_USER', 38),
+             ('mjSENS_PLUGIN', 38),
+             ('mjSENS_USER', 39),
          ]),
      )),
     ('mjtStage',
@@ -642,6 +643,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjFONT_BIG', 2),
          ]),
      )),
+    ('mjtTaskStatus',
+     EnumDecl(
+         name='mjtTaskStatus',
+         declname='enum mjtTaskStatus_',
+         values=dict([
+             ('mjTASK_NEW', 0),
+             ('mjTASK_QUEUED', 1),
+             ('mjTASK_COMPLETED', 2),
+         ]),
+     )),
     ('mjtButton',
      EnumDecl(
          name='mjtButton',

introspect/functions.py

@@ -8269,7 +8269,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  type=ValueType(name='size_t'),
              ),
          ),
-         doc='Creates a thread pool with the specified number of threads running.',  # pylint: disable=line-too-long
+         doc='Create a thread pool with the specified number of threads running.',  # pylint: disable=line-too-long
      )),
     ('mju_threadPoolEnqueue',
      FunctionDecl(
@@ -8288,32 +8288,8 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                      inner_type=ValueType(name='mjTask'),
                  ),
              ),
-             FunctionParameterDecl(
-                 name='start_routine',
-                 type=ValueType(name='void *(*)(void *)'),
-             ),
-             FunctionParameterDecl(
-                 name='args',
-                 type=PointerType(
-                     inner_type=ValueType(name='void'),
-                 ),
-             ),
          ),
-         doc='Enqueues a task in a thread pool.',
+         doc='Enqueue a task in a thread pool.',
-     )),
-    ('mju_taskJoin',
-     FunctionDecl(
-         name='mju_taskJoin',
-         return_type=ValueType(name='void'),
-         parameters=(
-             FunctionParameterDecl(
-                 name='task',
-                 type=PointerType(
-                     inner_type=ValueType(name='mjTask'),
-                 ),
-             ),
-         ),
-         doc='Waits for a task to complete.',
      )),
     ('mju_threadPoolDestroy',
      FunctionDecl(
@@ -8327,6 +8303,34 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  ),
              ),
          ),
-         doc='Destroys a thread pool.',
+         doc='Destroy a thread pool.',
+     )),
+    ('mju_defaultTask',
+     FunctionDecl(
+         name='mju_defaultTask',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='task',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjTask'),
+                 ),
+             ),
+         ),
+         doc='Initialize an mjTask.',
+     )),
+    ('mju_taskJoin',
+     FunctionDecl(
+         name='mju_taskJoin',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='task',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjTask'),
+                 ),
+             ),
+         ),
+         doc='Wait for a task to complete.',
      )),
 ])

introspect/structs.py

@@ -1885,6 +1885,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='global orientation in qpos0              (ncam x 9)',
              ),
+             StructFieldDecl(
+                 name='cam_resolution',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='[width, height] in pixels                (ncam x 2)',
+             ),
              StructFieldDecl(
                  name='cam_fovy',
                  type=PointerType(
@@ -2102,6 +2109,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='graph data address; -1: no graph         (nmesh x 1)',
              ),
+             StructFieldDecl(
+                 name='mesh_pos',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='translation applied to asset vertices    (nmesh x 3)',
+             ),
+             StructFieldDecl(
+                 name='mesh_quat',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+                 doc='rotation applied to asset vertices       (nmesh x 4)',
+             ),
              StructFieldDecl(
                  name='mesh_vert',
                  type=PointerType(
@@ -3562,19 +3583,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  name='solver',
                  type=ArrayType(
                      inner_type=ValueType(name='mjSolverStat'),
-                     extents=(1000,),
+                     extents=(4000,),
                  ),
-                 doc='solver statistics per iteration',
+                 doc='solver statistics per island, per iteration',
              ),
              StructFieldDecl(
-                 name='solver_iter',
+                 name='solver_nisland',
                  type=ValueType(name='int'),
-                 doc='number of solver iterations',
+                 doc='number of islands processed by solver',
+             ),
+             StructFieldDecl(
+                 name='solver_niter',
+                 type=ArrayType(
+                     inner_type=ValueType(name='int'),
+                     extents=(20,),
+                 ),
+                 doc='number of solver iterations, per island',
              ),
              StructFieldDecl(
                  name='solver_nnz',
-                 type=ValueType(name='int'),
+                 type=ArrayType(
-                 doc='number of non-zeros in Hessian or efc_AR',
+                     inner_type=ValueType(name='int'),
+                     extents=(20,),
+                 ),
+                 doc='number of non-zeros in Hessian or efc_AR, per island',
              ),
              StructFieldDecl(
                  name='solver_fwdinv',
@@ -6993,33 +7025,39 @@ STRUCTS: Mapping[str, StructDecl] = dict([
              ),
          ),
      )),
-    ('mjTask',
-     StructDecl(
-         name='mjTask',
-         declname='struct mjTask_',
-         fields=(
-             StructFieldDecl(
-                 name='buffer',
-                 type=ArrayType(
-                     inner_type=ValueType(name='char'),
-                     extents=(24,),
-                 ),
-                 doc='',
-             ),
-         ),
-     )),
     ('mjThreadPool',
      StructDecl(
          name='mjThreadPool',
          declname='struct mjThreadPool_',
          fields=(
              StructFieldDecl(
-                 name='buffer',
+                 name='nworker',
-                 type=ArrayType(
+                 type=ValueType(name='int'),
-                     inner_type=ValueType(name='char'),
+                 doc='number of workers in the pool',
-                     extents=(6208,),
+             ),
+         ),
+     )),
+    ('mjTask',
+     StructDecl(
+         name='mjTask',
+         declname='struct mjTask_',
+         fields=(
+             StructFieldDecl(
+                 name='func',
+                 type=ValueType(name='mjfTask'),
+                 doc='pointer to the function that implements the task',
+             ),
+             StructFieldDecl(
+                 name='args',
+                 type=PointerType(
+                     inner_type=ValueType(name='void'),
                  ),
-                 doc='',
+                 doc='arguments to func',
+             ),
+             StructFieldDecl(
+                 name='status',
+                 type=ValueType(name='int', is_volatile=True),
+                 doc='status of the task',
              ),
          ),
      )),

model/plugin/sdf/asset/README.md

@@ -0,0 +1,2 @@
+The spot assets were taken from https://www.cs.cmu.edu/~kmcrane/Projects/ModelRepository/ and are
+released under the CC0 1.0 Universal (CC0 1.0) Public Domain Dedication license.

model/plugin/sdf/cow.xml

@@ -0,0 +1,64 @@
+<mujoco>
+  <compiler texturedir="asset"/>
+
+  <extension>
+    <plugin plugin="mujoco.sdf.torus">
+      <instance name="torus">
+        <config key="radius1" value="0.15"/>
+        <config key="radius2" value="0.05"/>
+      </instance>
+    </plugin>
+    <plugin plugin="mujoco.sdf.sdflib">
+      <instance name="sdf">
+        <config key="aabb" value="0"/>
+      </instance>
+    </plugin>
+  </extension>
+
+  <asset>
+    <texture name="texspot" type="2d" file="spot.png"/>
+    <material name="matspot" texture="texspot"/>
+    <mesh name="spot" file="asset/spot.obj">
+      <plugin instance="sdf"/>
+    </mesh>
+    <mesh name="torus">
+      <plugin instance="torus"/>
+    </mesh>
+  </asset>
+
+  <option sdf_iterations="20" sdf_initpoints="40"/>
+
+  <visual>
+    <map force="1000"/>
+  </visual>
+
+  <default>
+    <geom solref="0.01 1" solimp=".95 .99 .0001" friction="0.5"/>
+  </default>
+
+  <statistic meansize="0.2"/>
+
+  <include file="scene.xml"/>
+
+  <worldbody>
+    <body pos="0.1 .25 5.7">
+      <freejoint/>
+      <geom type="sdf" mesh="torus" rgba=".2 .8 .2 1">
+        <plugin instance="torus"/>
+      </geom>
+    </body>
+    <body euler="90 0 0" pos="0 0 .7">
+      <geom type="sdf" name="cow1" mesh="spot" material="matspot">
+        <plugin instance="sdf"/>
+      </geom>
+    </body>
+    <body pos="0.05 .25 2.2">
+      <freejoint/>
+      <geom type="sdf" name="cow2" mesh="spot" material="matspot">
+        <plugin instance="sdf"/>
+      </geom>
+    </body>
+    <light name="left" pos="0 0 1"/>
+    <light name="right" pos="1 0 1"/>
+  </worldbody>
+</mujoco>

plugin/sdf/CMakeLists.txt

@@ -28,6 +28,8 @@ set(MUJOCO_SDF_SRCS
     register.cc
     nut.cc
     nut.h
+    sdflib.cc
+    sdflib.h
     torus.cc
     torus.h
 )
@@ -35,7 +37,7 @@ set(MUJOCO_SDF_SRCS
 add_library(sdf SHARED)
 target_sources(sdf PRIVATE ${MUJOCO_SDF_SRCS})
 target_include_directories(sdf PRIVATE ${MUJOCO_SDF_INCLUDE})
-target_link_libraries(sdf PRIVATE mujoco)
+target_link_libraries(sdf PRIVATE mujoco SdfLib)
 target_compile_options(
   sdf
   PRIVATE ${AVX_COMPILE_OPTIONS}

plugin/sdf/register.cc

@@ -17,6 +17,7 @@
 #include "gear.h"
 #include "nut.h"
 #include "torus.h"
+#include "sdflib.h"
 
 namespace mujoco::plugin::sdf {
 
@@ -26,6 +27,7 @@ mjPLUGIN_LIB_INIT {
   Gear::RegisterPlugin();
   Nut::RegisterPlugin();
   Torus::RegisterPlugin();
+  SdfLib::RegisterPlugin();
 }
 
 }  // namespace mujoco::plugin::sdf

plugin/sdf/sdflib.cc

@@ -0,0 +1,161 @@
+// 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 <cstdint>
+#include <optional>
+#include <utility>
+#include <vector>
+
+#include <SdfLib/utils/Mesh.h>
+#include <SdfLib/OctreeSdf.h>
+#include <mujoco/mjplugin.h>
+#include <mujoco/mujoco.h>
+#include "sdf.h"
+#include "sdflib.h"
+
+namespace mujoco::plugin::sdf {
+namespace {
+
+inline unsigned int* MakeNonConstUnsigned(const int* ptr) {
+  return reinterpret_cast<unsigned int*>(const_cast<int*>(ptr));
+}
+
+}  // namespace
+
+// factory function
+std::optional<SdfLib> SdfLib::Create(const mjModel* m, mjData* d,
+                                     int instance) {
+  int geomid = 0;
+  for (int i = 0; i < m->ngeom; ++i) {
+    if (m->geom_plugin[i] == instance) {
+      geomid = i;
+      break;
+    }
+  }
+  int meshid = m->geom_dataid[geomid];
+  int nvert = m->mesh_vertnum[meshid];
+  int nface = m->mesh_facenum[meshid];
+  int* indices = m->mesh_face + 3*m->mesh_faceadr[meshid];
+  float* verts = m->mesh_vert + 3*m->mesh_vertadr[meshid];
+  std::vector<glm::vec3> vertices(nvert);
+  for (int i = 0; i < nvert; i++) {
+    mjtNum vert[3] = {verts[3*i+0], verts[3*i+1], verts[3*i+2]};
+    mju_rotVecQuat(vert, vert, m->mesh_quat + 4*meshid);
+    mju_addTo3(vert, m->mesh_pos + 3*meshid);
+    vertices[i].x = vert[0];
+    vertices[i].y = vert[1];
+    vertices[i].z = vert[2];
+  }
+  sdflib::Mesh mesh(vertices.data(), nvert,
+                    MakeNonConstUnsigned(indices), 3*nface);
+  mesh.computeBoundingBox();
+  return SdfLib(std::move(mesh));
+}
+
+// plugin constructor
+SdfLib::SdfLib(sdflib::Mesh&& mesh) {
+  sdf_func_ =
+      sdflib::OctreeSdf(mesh, mesh.getBoundingBox(), 8, 3, 1e-3,
+                        sdflib::OctreeSdf::InitAlgorithm::CONTINUITY, 1);
+}
+
+// plugin computation
+void SdfLib::Compute(const mjModel* m, mjData* d, int instance) {
+  visualizer_.Next();
+}
+
+// plugin reset
+void SdfLib::Reset() {
+  visualizer_.Reset();
+}
+
+// plugin visualization
+void SdfLib::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
+                       mjvScene* scn, int instance) {
+  visualizer_.Visualize(m, d, opt, scn, instance);
+}
+
+// sdf
+mjtNum SdfLib::Distance(const mjtNum p[3]) const {
+  glm::vec3 point(p[0], p[1], p[2]);
+  return sdf_func_.getDistance(point);
+}
+
+// gradient of sdf
+void SdfLib::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
+  glm::vec3 gradient;
+  glm::vec3 p(point[0], point[1], point[2]);
+  sdf_func_.getDistance(p, gradient);
+  grad[0] = gradient[0];
+  grad[1] = gradient[1];
+  grad[2] = gradient[2];
+}
+
+// plugin registration
+void SdfLib::RegisterPlugin() {
+  mjpPlugin plugin;
+  mjp_defaultPlugin(&plugin);
+
+  plugin.name = "mujoco.sdf.sdflib";
+  plugin.capabilityflags |= mjPLUGIN_SDF;
+
+  const char* attributes[] = {"aabb"};
+  plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
+  plugin.attributes = attributes;
+  plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
+
+  plugin.init = +[](const mjModel* m, mjData* d, int instance) {
+    auto sdf_or_null = SdfLib::Create(m, d, instance);
+    if (!sdf_or_null.has_value()) {
+      return -1;
+    }
+    d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
+        new SdfLib(std::move(*sdf_or_null)));
+    return 0;
+  };
+  plugin.destroy = +[](mjData* d, int instance) {
+    delete reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
+    d->plugin_data[instance] = 0;
+  };
+  plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
+                     int instance) {
+    auto sdf = reinterpret_cast<SdfLib*>(plugin_data);
+    sdf->Reset();
+  };
+  plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
+                         mjvScene* scn, int instance) {
+    auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
+    sdf->Visualize(m, d, opt, scn, instance);
+  };
+  plugin.compute =
+      +[](const mjModel* m, mjData* d, int instance, int capability_bit) {
+        auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
+        sdf->Compute(m, d, instance);
+      };
+  plugin.sdf_distance =
+      +[](const mjtNum point[3], const mjData* d, int instance) {
+        auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
+        sdf->visualizer_.AddPoint(point);
+        return sdf->Distance(point);
+      };
+  plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
+                        const mjData* d, int instance) {
+    auto* sdf = reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
+    sdf->Gradient(gradient, point);
+  };
+
+  mjp_registerPlugin(&plugin);
+}
+
+}  // namespace mujoco::plugin::sdf

plugin/sdf/sdflib.h

@@ -0,0 +1,54 @@
+// 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_PLUGIN_SDF_SDFLIB_H_
+#define MUJOCO_PLUGIN_SDF_SDFLIB_H_
+
+#include <optional>
+
+#include <SdfLib/utils/Mesh.h>
+#include <SdfLib/OctreeSdf.h>
+#include <mujoco/mjdata.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mjtnum.h>
+#include <mujoco/mjvisualize.h>
+#include "sdf.h"
+
+namespace mujoco::plugin::sdf {
+class SdfLib {
+ public:
+  // Creates a new SdfLib instance or returns null on failure.
+  static std::optional<SdfLib> Create(const mjModel* m, mjData* d,
+                                      int instance);
+  SdfLib(SdfLib&&) = default;
+  ~SdfLib() = default;
+
+  void Reset();
+  void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
+                 mjvScene* scn, int instance);
+  void Compute(const mjModel* m, mjData* d, int instance);
+  mjtNum Distance(const mjtNum point[3]) const;
+  void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
+
+  static void RegisterPlugin();
+
+ private:
+  SdfLib(sdflib::Mesh&& mesh);
+  SdfVisualizer visualizer_;
+  sdflib::OctreeSdf sdf_func_;
+};
+
+}  // namespace mujoco::plugin::sdf
+
+#endif  // MUJOCO_PLUGIN_SDF_SDFLIB_H_

python/mujoco/indexer_xmacro.h

@@ -281,7 +281,8 @@
   X( mjtNum, key_, qvel,  nkey, MJ_M(nv)       ) \
   X( mjtNum, key_, act,   nkey, MJ_M(na)       ) \
   X( mjtNum, key_, mpos,  nkey, MJ_M(nmocap)*3 ) \
-  X( mjtNum, key_, mquat, nkey, MJ_M(nmocap)*4 )
+  X( mjtNum, key_, mquat, nkey, MJ_M(nmocap)*4 ) \
+  X( mjtNum, key_, ctrl,  nkey, MJ_M(nu)       )
 
 #define MJMODEL_VIEW_GROUPS                                            \
   XGROUP( MjModelActuatorViews, actuator, nu,       MJMODEL_ACTUATOR ) \

python/mujoco/structs.h

@@ -582,6 +582,8 @@ class MjWrapper<raw::MjData>: public WrapperBase<raw::MjData> {
   py_array_or_tuple_t<raw::MjWarningStat> warning;
   py_array_or_tuple_t<raw::MjTimerStat> timer;
   py_array_or_tuple_t<raw::MjSolverStat> solver;
+  py_array_or_tuple_t<int> solver_niter;
+  py_array_or_tuple_t<int> solver_nnz;
   py_array_or_tuple_t<mjtNum> solver_fwdinv;
   py_array_or_tuple_t<mjtNum> energy;
 

simulate/main.cc

@@ -437,7 +437,8 @@ __attribute__((used, visibility("default"))) extern "C" void _mj_rosettaError(co
 #endif
 
 // run event loop
-int main(int argc, const char** argv) {
+int main(int argc, char** argv) {
+
   // display an error if running on macOS under Rosetta 2
 #if defined(__APPLE__) && defined(__AVX__)
   if (rosetta_error_msg) {

simulate/simulate.cc

@@ -14,6 +14,7 @@
 
 #include "simulate.h"
 
+#include <algorithm>
 #include <atomic>
 #include <chrono>
 #include <cstdio>
@@ -175,6 +176,10 @@ const char help_title[] =
 
 //-------------------------------- 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
@@ -211,6 +216,20 @@ void InitializeProfiler(mj::Simulate* sim) {
   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; i<mjMAXLINE; i++) {
+    fig->linergb[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; i<mjMAXLINE; i++) {
+    fig->linergb[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");
@@ -271,54 +290,76 @@ void InitializeProfiler(mj::Simulate* sim) {
 
 // update profiler figures
 void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
-  // update constraint figure
+  // reset lines in Constraint and Cost figures
-  sim->figconstraint.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
+  memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int));
-  for (int i=1; i<5; i++) {
+  memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int));
-    sim->figconstraint.linepnt[i] = sim->figconstraint.linepnt[0];
-  }
-  if (m->opt.solver==mjSOL_PGS) {
-    sim->figconstraint.linepnt[3] = 0;
-    sim->figconstraint.linepnt[4] = 0;
-  }
-  if (m->opt.solver==mjSOL_CG) {
-    sim->figconstraint.linepnt[4] = 0;
-  }
-  for (int i=0; i<sim->figconstraint.linepnt[0]; i++) {
-    // x
-    sim->figconstraint.linedata[0][2*i] = i;
-    sim->figconstraint.linedata[1][2*i] = i;
-    sim->figconstraint.linedata[2][2*i] = i;
-    sim->figconstraint.linedata[3][2*i] = i;
-    sim->figconstraint.linedata[4][2*i] = i;
 
-    // y
+  // number of islands that have diagnostics
-    sim->figconstraint.linedata[0][2*i+1] = d->nefc;
+  int nisland = mjMIN(d->solver_nisland, mjNISLAND);
-    sim->figconstraint.linedata[1][2*i+1] = d->solver[i].nactive;
-    sim->figconstraint.linedata[2][2*i+1] = d->solver[i].nchange;
-    sim->figconstraint.linedata[3][2*i+1] = d->solver[i].neval;
-    sim->figconstraint.linedata[4][2*i+1] = d->solver[i].nupdate;
-  }
 
-  // update cost figure
+  // iterate over islands
-  sim->figcost.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
+  for (int k=0; k < nisland; k++) {
-  for (int i=1; i<3; i++) {
+    // ==== update Constraint ("Counts") figure
-    sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
-  }
-  if (m->opt.solver==mjSOL_PGS) {
-    sim->figcost.linepnt[1] = 0;
-    sim->figcost.linepnt[2] = 0;
-  }
 
-  for (int i=0; i<sim->figcost.linepnt[0]; i++) {
+    // number of points to plot, starting line
-    // x
+    int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT);
-    sim->figcost.linedata[0][2*i] = i;
+    int start = kConstraintNum * k;
-    sim->figcost.linedata[1][2*i] = i;
-    sim->figcost.linedata[2][2*i] = i;
 
-    // y
+    sim->figconstraint.linepnt[start + 0] = npoints;
-    sim->figcost.linedata[0][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].improvement));
+    for (int i=1; i < kConstraintNum; i++) {
-    sim->figcost.linedata[1][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].gradient));
+      sim->figconstraint.linepnt[start + i] = npoints;
-    sim->figcost.linedata[2][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].lineslope));
+    }
+    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; i<npoints; i++) {
+      // x
+      sim->figconstraint.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_efcnum[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; i<kCostNum; i++) {
+      sim->figcost.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; i<sim->figcost.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
@@ -354,14 +395,22 @@ void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
     }
   }
 
+  // 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<float>(m->nv),
     static_cast<float>(m->nbody),
     static_cast<float>(d->nefc),
-    static_cast<float>(mju_sqrt(d->solver_nnz)),
+    static_cast<float>(sqrt_nnz),
     static_cast<float>(d->ncon),
-    static_cast<float>(d->solver_iter)
+    static_cast<float>(solver_niter)
   };
 
   // update figsize
@@ -496,14 +545,22 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
               char (&content)[mj::Simulate::kMaxFilenameLength]) {
   char tmp[20];
 
-  // compute solver error
+  // number of islands with statistics
+  int nisland = mjMIN(d->solver_nisland, mjNISLAND);
+
+  // compute solver error (maximum over islands)
   mjtNum solerr = 0;
-  if (d->solver_iter) {
+  for (int i=0; i < nisland; i++) {
-    int ind = mjMIN(d->solver_iter-1, mjNSOLVER-1);
+    mjtNum solerr_i = 0;
-    solerr = mju_min(d->solver[ind].improvement, d->solver[ind].gradient);
+    if (d->solver_niter[i]) {
-    if (solerr==0) {
+      int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1;
-      solerr = mju_max(d->solver[ind].improvement, d->solver[ind].gradient);
+      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));
 
@@ -515,6 +572,12 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
     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,
@@ -524,7 +587,7 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
                    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, d->solver_iter,
+                   solerr, solver_niter,
                    fps,
                    d->maxuse_arena/(double)(d->narena),
                    mju_writeNumBytes(d->narena));
@@ -1638,8 +1701,8 @@ void Simulate::Sync() {
     return;
   }
 
-  bool update_profiler = this->profiler && (this->run || !this->m_);
+  bool update_profiler = this->profiler && (this->pause_update || this->run);
-  bool update_sensor = this->sensor && (this->run || !this->m_);
+  bool update_sensor = this->sensor && (this->pause_update || this->run);
 
   for (int i = 0; i < m_->njnt; ++i) {
     std::optional<std::pair<mjtNum, mjtNum>> range;

simulate/simulate.h

@@ -155,6 +155,7 @@ class Simulate {
   int info = 0;
   int profiler = 0;
   int sensor = 0;
+  int pause_update = 1;
   int fullscreen = 0;
   int vsync = 1;
   int busywait = 0;
@@ -187,7 +188,7 @@ class Simulate {
   int real_time_index = 0;
   bool speed_changed = true;
   float measured_slowdown = 1.0;
-  // logarithmically spaced realtime slow-down coefficients (percent)
+  // 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,
@@ -232,7 +233,7 @@ class Simulate {
 
   // 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[14] = {
+  const mjuiDef def_option[15] = {
     {mjITEM_SECTION,  "Option",        1, nullptr,           "AO"},
     {mjITEM_SELECT,   "Spacing",       1, &this->spacing,    "Tight\nWide"},
     {mjITEM_SELECT,   "Color",         1, &this->color,      "Default\nOrange\nWhite\nBlack"},
@@ -243,6 +244,7 @@ class Simulate {
     {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

src/engine/CMakeLists.txt

@@ -29,7 +29,7 @@ set(MUJOCO_ENGINE_SRCS
     engine_core_constraint.h
     engine_core_smooth.c
     engine_core_smooth.h
-    engine_crossplatform.c
+    engine_crossplatform.cc
     engine_crossplatform.h
     engine_derivative.c
     engine_derivative.h

src/engine/engine_collision_sdf.c

@@ -259,8 +259,8 @@ static void undoTransformation(const mjModel* m, const mjData* d, int g,
   mjtNum* xmat = d->geom_xmat + 9 * g;
   if (m->geom_type[g]==mjGEOM_MESH || m->geom_type[g]==mjGEOM_SDF) {
     mjtNum negpos[3], negquat[4], xquat[4];
-    mjtNum* pos = m->geom_pos + 3 * g;
+    mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g];
-    mjtNum* quat = m->geom_quat + 4 * g;
+    mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g];
     mju_mat2Quat(xquat, xmat);
     mju_negPose(negpos, negquat, pos, quat);
     mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
@@ -367,7 +367,9 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
     mjc_gradient(m, d, s, grad, x);
 
     // sanity check
-    if (isnan(grad[0]) || isnan(grad[1]) || isnan(grad[2])) {
+    if (isnan(grad[0]) || grad[0]>mjMAXVAL || grad[0]<-mjMAXVAL ||
+        isnan(grad[1]) || grad[1]>mjMAXVAL || grad[1]<-mjMAXVAL ||
+        isnan(grad[2]) || grad[2]>mjMAXVAL || grad[2]<-mjMAXVAL) {
       return mjMAXVAL;
     }
 

src/engine/engine_core_smooth.c

@@ -1212,11 +1212,17 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
 // in-place sparse backsubstitution for one island:  x = inv(L'*D*L)*x
 //  L is in lower triangle of qLD; D is on diagonal of qLD
 void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int island) {
+  // if no islands, call mj_solveLD
+  const mjtNum* qLD = d->qLD;
+  const mjtNum* qLDiagInv = d->qLDiagInv;
+  if (island < 0) {
+    mj_solveLD(m, x, 1, qLD, qLDiagInv);
+    return;
+  }
+
   // local constants: general
   const int* Madr = m->dof_Madr;
   const int* parentid = m->dof_parentid;
-  const mjtNum* qLD = d->qLD;
-  const mjtNum* qLDiagInv = d->qLDiagInv;
   const int* simplenum = m->dof_simplenum;
 
   // local constants: island specific

src/engine/engine_crossplatform.c

@@ -1,41 +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.
-
-void _mj_crossplatform_void(void) {}  // ISO C does not permit empty translation units
-
-#if defined(__APPLE__) && defined(__AVX__)
-
-#include <stdio.h>
-#include <string.h>
-#include <sys/sysctl.h>
-
-__attribute__((weak, visibility("default"))) void _mj_rosettaError(const char* msg) {
-  fprintf(stderr, "%s\n", msg);
-  __asm__ __volatile__ ("ud2");  // raises SIGILL but leave this function at the top of the stack
-}
-
-__attribute__((constructor(10000), target("no-avx"))) static void _mj_checkRosetta(void) {
-  int is_translated = 0;
-  {
-    size_t len = sizeof(is_translated);
-    if (sysctlbyname("sysctl.proc_translated", &is_translated, &len, NULL, 0)) {
-      is_translated = 0;
-    }
-  }
-  if (is_translated) {
-    _mj_rosettaError("MuJoCo cannot be run under Rosetta 2 on an Apple Silicon machine.");
-  }
-}
-
-#endif  // defined(__APPLE__) && defined(__AVX__)

src/engine/engine_crossplatform.cc

@@ -0,0 +1,99 @@
+// 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 "engine/engine_crossplatform.h"  // IWYU pragma: keep
+
+#if defined(__APPLE__) && defined(__AVX__)
+#include <sys/sysctl.h>
+
+#include <cstdio>
+#include <cstring>
+
+namespace {
+__attribute__((weak, visibility("default")))
+extern "C" void _mj_rosettaError(const char* msg) {
+  fprintf(stderr, "%s\n", msg);
+  __asm__ __volatile__ ("ud2");  // raises SIGILL but leave this function at the top of the stack
+}
+
+__attribute__((constructor(10000), target("no-avx")))
+void CheckRosetta() {
+  int is_translated = 0;
+  {
+    size_t len = sizeof(is_translated);
+    if (sysctlbyname("sysctl.proc_translated", &is_translated, &len, NULL, 0)) {
+      is_translated = 0;
+    }
+  }
+  if (is_translated) {
+    _mj_rosettaError("MuJoCo cannot be run under Rosetta 2 on an Apple Silicon machine.");
+  }
+}
+}  // namespace
+#endif  // defined(__APPLE__) && defined(__AVX__)
+
+#ifdef ADDRESS_SANITIZER
+#include <sanitizer/common_interface_defs.h>
+
+#include <array>
+#include <map>
+#include <mutex>
+#include <shared_mutex>
+#include <string>
+#include <string_view>
+#include <unordered_map>
+#include <utility>
+
+namespace {
+std::string_view SymbolizeCached(void* pc) {
+  static auto* mu = new std::shared_mutex;
+  static auto* pc_to_func_name_map = new std::unordered_map<void*, std::string>;
+
+  {
+    std::shared_lock lock(*mu);
+    auto it = pc_to_func_name_map->find(pc);
+    if (it != pc_to_func_name_map->end()) {
+      return it->second;
+    }
+  }
+
+  std::array<char, 256> buf;
+  __sanitizer_symbolize_pc(pc, "%f", buf.data(), buf.size());
+  {
+    std::unique_lock lock(*mu);
+    return pc_to_func_name_map->emplace(pc, buf.data()).first->second;
+  }
+}
+}  // namespace
+
+int _mj_comparePcFuncName(void* pc1, void* pc2) {
+  static auto* mu = new std::shared_mutex;
+  static auto* same_func_map = new std::map<std::pair<void*, void*>, bool>;
+
+  auto pc_pair = std::make_pair(pc1, pc2);
+  {
+    std::shared_lock lock(*mu);
+    auto it = same_func_map->find(pc_pair);
+    if (it != same_func_map->end()) {
+      return it->second;
+    }
+  }
+
+  bool is_same = (SymbolizeCached(pc1) == SymbolizeCached(pc2));
+  {
+    std::unique_lock lock(*mu);
+    return same_func_map->emplace(pc_pair, is_same).first->second;
+  }
+}
+#endif  // ADDRESS_SANITIZER

src/engine/engine_crossplatform.h

@@ -75,4 +75,16 @@
   #define mjUNLIKELY(x) (x)
 #endif
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef ADDRESS_SANITIZER
+int _mj_comparePcFuncName(void* pc1, void* pc2);
+#endif
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
 #endif  // MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_

src/engine/engine_forward.c

@@ -264,7 +264,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
           mjcb_act_dyn(m, d, i);
         }
       } else {
-        d->act_dot[j] = 0;
+        mju_zero(d->act_dot + j, m->actuator_actnum[i]);
       }
     }
   }
@@ -501,7 +501,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
     mju_copy(d->qacc, d->qacc_smooth, nv);
     mju_copy(d->qacc_warmstart, d->qacc_smooth, nv);
     mju_zero(d->qfrc_constraint, nv);
-    d->solver_iter = 0;
+    mju_zeroInt(d->solver_niter, mjNISLAND);
     TM_END(mjTIMER_CONSTRAINT);
     return;
   }
@@ -512,7 +512,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
 
   // warmstart solver
   warmstart(m, d);
-  d->solver_iter = 0;
+  mju_zeroInt(d->solver_niter, mjNISLAND);
 
   // run main solver
   switch ((mjtSolver) m->opt.solver) {
@@ -532,6 +532,9 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
     mjERROR("unknown solver type %d", m->opt.solver);
   }
 
+  // one (monolithic) island
+  d->solver_nisland = 1;
+
   // save result for next step warmstart
   mju_copy(d->qacc_warmstart, d->qacc, nv);
 
@@ -551,7 +554,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
 static void mj_advance(const mjModel* m, mjData* d,
                        const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
   // advance activations and clamp
-  if (m->na) {
+  if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
     for (int i=0; i < m->nu; i++) {
       int actadr = m->actuator_actadr[i];
       int actadr_end = actadr + m->actuator_actnum[i];

src/engine/engine_io.c

@@ -1351,29 +1351,17 @@ void mj_freeStack(mjData* d) {
 
   mjStackFrame* s = (mjStackFrame*) ((char*)d->arena + d->narena - d->pbase);
 #ifdef ADDRESS_SANITIZER
-  #define mjSYMBOLIZELEN 256
-
-  // symbolize s->pc to get the function name of most recent caller to mj_markStack
-  char markstack_func[mjSYMBOLIZELEN];
-  __sanitizer_symbolize_pc(s->pc, "%f", markstack_func, mjSYMBOLIZELEN);
-  markstack_func[mjSYMBOLIZELEN - 1] = '\0';
-
-  // symbolize current program counter to get the function name of caller to this function
-  char freestack_func[mjSYMBOLIZELEN];
-  __sanitizer_symbolize_pc(__sanitizer_return_address(), "%f", freestack_func, mjSYMBOLIZELEN);
-  freestack_func[mjSYMBOLIZELEN - 1] = '\0';
-
   // raise an error if caller function name doesn't match the most recent caller of mj_markStack
-  if (strncmp(markstack_func, freestack_func, mjSYMBOLIZELEN)) {
+  if (!_mj_comparePcFuncName(s->pc, __sanitizer_return_address())) {
+    #define mjSYMBOLIZELEN 256
     char dbginfo[mjSYMBOLIZELEN];
     __sanitizer_symbolize_pc(
         s->pc, "mj_markStack %F at %S has no corresponding mj_freeStack",
         dbginfo, sizeof(dbginfo));
     dbginfo[mjSYMBOLIZELEN - 1] = '\0';
     mjERROR("%s", dbginfo);
+    #undef mjSYMBOLIZELEN
   }
-
-  #undef mjSYMBOLIZELEN
 #endif
 
   // restore pbase and pstack
@@ -1440,9 +1428,10 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   // clear solver diagnostics
   memset(d->warning, 0, mjNWARNING*sizeof(mjWarningStat));
   memset(d->timer, 0, mjNTIMER*sizeof(mjTimerStat));
-  memset(d->solver, 0, mjNSOLVER*sizeof(mjSolverStat));
+  memset(d->solver, 0, mjNSOLVER*mjNISLAND*sizeof(mjSolverStat));
-  d->solver_iter = 0;
+  d->solver_nisland = 0;
-  d->solver_nnz = 0;
+  mju_zeroInt(d->solver_niter, mjNISLAND);
+  mju_zeroInt(d->solver_nnz, mjNISLAND);
   mju_zero(d->solver_fwdinv, 2);
 
   // clear collision diagnostics
@@ -1624,6 +1613,9 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
   case mjSENS_CLOCK:
     return 1;
 
+  case mjSENS_CAMPROJECTION:
+    return 2;
+
   case mjSENS_ACCELEROMETER:
   case mjSENS_VELOCIMETER:
   case mjSENS_GYRO:

src/engine/engine_print.c

@@ -837,24 +837,33 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
   }
 
   // SOLVER STAT
-  if (d->solver_iter) {
+  if (d->nefc) {
     fprintf(fp, "SOLVER STAT\n");
-    fprintf(fp, "  solver_iter = %d\n", d->solver_iter);
+    fprintf(fp, "  solver_nisland = %d\n", d->solver_nisland);
-    fprintf(fp, "  solver_nnz = %d\n",  d->solver_nnz);
+    printVector("  solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
-    for (int i=0; i < mjMIN(mjNSOLVER, d->solver_iter); i++) {
+    int nisland_stat = mjMIN(d->solver_nisland, mjNISLAND);
-      fprintf(fp, "    %d:  improvement = ", i);
+    for (int island=0; island < nisland_stat; island++) {
-      fprintf(fp, float_format, d->solver[i].improvement);
+      int niter_stat = mjMIN(mjNSOLVER, d->solver_niter[island]);
-      fprintf(fp, "  gradient = ");
+      if (niter_stat) {
-      fprintf(fp, float_format, d->solver[i].gradient);
+        fprintf(fp, "  ISLAND %d\n", island);
-      fprintf(fp, "  lineslope = ");
+        fprintf(fp, "    solver_niter = %d\n", d->solver_niter[island]);
-      fprintf(fp, float_format, d->solver[i].lineslope);
+        fprintf(fp, "    solver_nnz = %d\n",  d->solver_nnz[island]);
-      fprintf(fp, "\n");
+        for (int i=0; i < niter_stat; i++) {
-      fprintf(fp, "        nactive = %d   nchange = %d   neval = %d   nupdate = %d\n",
+          mjSolverStat* stat = d->solver + island*mjNSOLVER + i;
-              d->solver[i].nactive, d->solver[i].nchange,
+          fprintf(fp, "      %d:  improvement = ", i);
-              d->solver[i].neval, d->solver[i].nupdate);
+          fprintf(fp, float_format, stat->improvement);
+          fprintf(fp, "    gradient = ");
+          fprintf(fp, float_format, stat->gradient);
+          fprintf(fp, "    lineslope = ");
+          fprintf(fp, float_format, stat->lineslope);
+          fprintf(fp, "\n");
+          fprintf(fp, "          nactive = %d   nchange = %d   neval = %d   nupdate = %d\n",
+                  stat->nactive, stat->nchange,
+                  stat->neval, stat->nupdate);
+        }
+        fprintf(fp, "\n");
+      }
     }
-    printVector("solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
-    fprintf(fp, "\n");
   }
 
   printVector("ENERGY = ", d->energy, 2, fp, float_format);

src/engine/engine_ray.c

@@ -774,6 +774,9 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
     if (distance < 1e-8) {
       return distance_total;
     }
+    if (distance > 1e6) {
+      break;
+    }
   }
 
   // reset counter

src/engine/engine_sensor.c

@@ -187,6 +187,91 @@ static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, in
 }
 
 
+static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
+                        const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
+                        const int cam_res[2], mjtNum cam_fovy) {
+  // translation matrix (4x4)
+  mjtNum translation[4][4] = {0};
+  translation[0][0] = 1;
+  translation[1][1] = 1;
+  translation[2][2] = 1;
+  translation[3][3] = 1;
+  translation[0][3] = -cam_xpos[0];
+  translation[1][3] = -cam_xpos[1];
+  translation[2][3] = -cam_xpos[2];
+
+  // rotation matrix (4x4)
+  mjtNum rotation[4][4] = {0};
+  rotation[0][0] = 1;
+  rotation[1][1] = 1;
+  rotation[2][2] = 1;
+  rotation[3][3] = 1;
+  for (int i=0; i<3; i++) {
+    for (int j=0; j<3; j++) {
+      rotation[i][j] = cam_xmat[j*3+i];
+    }
+  }
+
+  // focal transformation matrix (3x4)
+  mjtNum height = (mjtNum) cam_res[1];
+  mjtNum fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * height;
+  mjtNum focal[3][4] = {0};
+  focal[0][0] = -fy;
+  focal[1][1] =  fy;
+  focal[2][2] = 1.0;
+
+  // image matrix (3x3)
+  mjtNum image[3][3] = {0};
+  image[0][0] = 1;
+  image[1][1] = 1;
+  image[2][2] = 1;
+  image[0][2] = (mjtNum)cam_res[0] / 2.0;
+  image[1][2] = (mjtNum)cam_res[1] / 2.0;
+
+  // projection matrix (3x4): product of all 4 matrices
+  mjtNum proj[3][4] = {0};
+  for (int i=0; i<3; i++) {
+    for (int j=0; j<3; j++) {
+      for (int k=0; k<4; k++) {
+        for (int l=0; l<4; l++) {
+          for (int n=0; n<4; n++) {
+            proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n];
+          }
+        }
+      }
+    }
+  }
+
+  // projection matrix multiplies homogenous [x, y, z, 1] vectors
+  mjtNum pos_hom[4] = {0, 0, 0, 1};
+  mju_copy3(pos_hom, target_xpos);
+
+  // project world coordinates into pixel space, see:
+  // https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
+  mjtNum pixel_coord_hom[3] = {0};
+  for (int i=0; i<3; i++) {
+      for (int j=0; j<4; j++) {
+        pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
+      }
+  }
+
+  // avoid dividing by tiny numbers
+  mjtNum denom = pixel_coord_hom[2];
+  if (mju_abs(denom) < mjMINVAL) {
+    if (denom < 0) {
+      denom = mju_min(denom, -mjMINVAL);
+    } else {
+      denom = mju_max(denom, mjMINVAL);
+    }
+  }
+
+  // compute projection
+  sensordata[0] = pixel_coord_hom[0] / denom;
+  sensordata[1] = pixel_coord_hom[1] / denom;
+}
+
+
+
 //-------------------------------- sensor ----------------------------------------------------------
 
 // position-dependent sensors
@@ -221,6 +306,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
         mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
         break;
 
+      case mjSENS_CAMPROJECTION:                          // camera projection
+        cam_project(d->sensordata+adr, d->site_xpos+3*objid, d->cam_xpos+3*refid,
+                    d->cam_xmat+9*refid, m->cam_resolution+2*refid, m->cam_fovy[refid]);
+        break;
+
       case mjSENS_RANGEFINDER:                            // rangefinder
         rvec[0] = d->site_xmat[9*objid+2];
         rvec[1] = d->site_xmat[9*objid+5];

src/engine/engine_solver.c

@@ -39,24 +39,30 @@ static mjtNum rescale(const mjModel* m, mjtNum x) {
 
 
 
-// save solver statistics, count
+// save solver statistics
-static void saveStats(const mjModel* m, mjData* d, int* piter,
+static void saveStats(const mjModel* m, mjData* d, int island, int iter,
                       mjtNum improvement, mjtNum gradient, mjtNum lineslope,
                       int nactive, int nchange, int neval, int nupdate) {
-  // compute position, increase iter
+  // if out of range, return
-  int i = d->solver_iter + (*piter);
+  if (island >= mjNISLAND) {
-  (*piter)++;
+    return;
-
-  // save if within range
-  if (i < mjNSOLVER) {
-    d->solver[i].improvement = improvement;
-    d->solver[i].gradient = gradient;
-    d->solver[i].lineslope = lineslope;
-    d->solver[i].nactive = nactive;
-    d->solver[i].nchange = nchange;
-    d->solver[i].neval = neval;
-    d->solver[i].nupdate = nupdate;
   }
+
+  // if no islands, use first island
+  island = mjMAX(0, island);
+
+  // get mjSolverStat pointer
+  iter += d->solver_niter[island];  // add current niter (in case of noslip)
+  mjSolverStat* stat = d->solver + island*mjNSOLVER + iter;
+
+  // save stats
+  stat->improvement = improvement;
+  stat->gradient = gradient;
+  stat->lineslope = lineslope;
+  stat->nactive = nactive;
+  stat->nchange = nchange;
+  stat->neval = neval;
+  stat->nupdate = nupdate;
 }
 
 
@@ -313,6 +319,9 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
   mjtNum* ARinv = mj_stackAllocNum(d, nefc);
   int* oldstate = mj_stackAllocInt(d, nefc);
 
+  // TODO: b/295296178 - Use island index (currently hardcoded to 0)
+  int island = 0;
+
   // precompute inverse diagonal of AR
   ARdiaginv(m, d, ARinv, 0);
 
@@ -471,9 +480,13 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
       nchange += (oldstate[i] != d->efc_state[i]);
     }
 
-    // scale improvement, save stats, count
+    // scale improvement, save stats
     improvement = rescale(m, improvement);
-    saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0);
+    saveStats(m, d, island, iter, improvement, 0, 0, nactive, nchange, 0, 0);
+
+    // increment iteration count
+    iter++;
+
 
     // terminate
     if (improvement < m->opt.tolerance) {
@@ -481,17 +494,20 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
     }
   }
 
-  // update solver iterations
+  // finalize statistics
-  d->solver_iter += iter;
+  if (island < mjNISLAND) {
+    // update solver iterations
+    d->solver_niter[island] += iter;
 
-  // set nnz
+    // set nnz
-  if (mj_isSparse(m)) {
+    if (mj_isSparse(m)) {
-    d->solver_nnz = 0;
+      d->solver_nnz[island] = 0;
-    for (int i=0; i < nefc; i++) {
+      for (int i=0; i < nefc; i++) {
-      d->solver_nnz += d->efc_AR_rownnz[i];
+        d->solver_nnz[island] += d->efc_AR_rownnz[i];
+      }
+    } else {
+      d->solver_nnz[island] = nefc*nefc;
     }
-  } else {
-    d->solver_nnz = nefc*nefc;
   }
 
   // map to joint space
@@ -515,6 +531,9 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
   mjtNum* ARinv = mj_stackAllocNum(d, nefc);
   int* oldstate = mj_stackAllocInt(d, nefc);
 
+  // TODO: b/295296178 - Use island index (currently hardcoded to 0)
+  int island = 0;
+
   // precompute inverse diagonal of A
   ARdiaginv(m, d, ARinv, 1);
 
@@ -687,9 +706,12 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
       nchange += (oldstate[i] != d->efc_state[i]);
     }
 
-    // scale improvement, save stats, count
+    // scale improvement, save stats
     improvement = rescale(m, improvement);
-    saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0);
+    saveStats(m, d, island, iter, improvement, 0, 0, nactive, nchange, 0, 0);
+
+    // increment iteration count
+    iter++;
 
     // terminate
     if (improvement < m->opt.noslip_tolerance) {
@@ -698,7 +720,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
   }
 
   // update solver iterations
-  d->solver_iter += iter;
+  d->solver_niter[island] += iter;
 
   // map to joint space
   dualFinish(m, d);
@@ -1514,6 +1536,9 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
   mjCGContext ctx;
   mj_markStack(d);
 
+  // TODO: b/295296178 - Use island index (currently hardcoded to 0)
+  int island = 0;
+
   // allocate context
   CGallocate(m, d, &ctx, flg_Newton);
 
@@ -1576,12 +1601,15 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
       nchange += (d->efc_state[i] != oldstate[i]);
     }
 
-    // scale improvement, save stats, count
+    // scale improvement, save stats
     mjtNum improvement = rescale(m, oldcost-ctx.cost);
     mjtNum gradient = rescale(m, mju_norm(ctx.grad, nv));
-    saveStats(m, d, &iter, improvement, gradient, ctx.LSslope,
+    saveStats(m, d, island, iter, improvement, gradient, ctx.LSslope,
               ctx.nactive, nchange, ctx.LSiter, ctx.nupdate);
 
+    // increment iteration count
+    iter++;
+
     // termination
     if (improvement < m->opt.tolerance || gradient < m->opt.tolerance) {
       break;
@@ -1608,18 +1636,21 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
     }
   }
 
-  // update solver iterations
+  // finalize statistics
-  d->solver_iter += iter;
+  if (island < mjNISLAND) {
+    // update solver iterations
+    d->solver_niter[island] += iter;
 
-  // set solver_nnz
+    // set solver_nnz
-  if (flg_Newton) {
+    if (flg_Newton) {
-    if (mj_isSparse(m)) {
+      if (mj_isSparse(m)) {
-      d->solver_nnz = 2*ctx.nnz - nv;
+        d->solver_nnz[island] = 2*ctx.nnz - nv;
+      } else {
+        d->solver_nnz[island] = nv*nv;
+      }
     } else {
-      d->solver_nnz = nv*nv;
+      d->solver_nnz[island] = 0;
     }
-  } else {
-    d->solver_nnz = 0;
   }
 
   mj_freeStack(d);

src/engine/engine_util_blas.c

@@ -220,7 +220,7 @@ mjtNum mju_normalize4(mjtNum vec[4]) {
     vec[1] = 0;
     vec[2] = 0;
     vec[3] = 0;
-  } else {
+  } else if (mju_abs(norm - 1) > mjMINVAL) {
     mjtNum normInv = 1/norm;
     vec[0] *= normInv;
     vec[1] *= normInv;

src/thread/CMakeLists.txt

@@ -13,11 +13,11 @@
 # limitations under the License.
 
 set(MUJOCO_THREAD_SRCS
-    lockless_queue.h
-    task.cc
-    task.h
     thread_pool.cc
     thread_pool.h
+    thread_queue.h
+    thread_task.cc
+    thread_task.h
 )
 
 target_sources(mujoco PRIVATE ${MUJOCO_THREAD_SRCS})

src/thread/task.h

@@ -1,70 +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.
-// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
-// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
-
-#ifndef MUJOCO_SRC_THREAD_TASK_H_
-#define MUJOCO_SRC_THREAD_TASK_H_
-
-#ifdef __cplusplus
-
-#include <atomic>
-#include <new>
-#include <thread>
-
-namespace mujoco {
-
-class Task {
- public:
-  using FunctionPtr = void* (*)(void*);
-  enum Status {
-    QUEUED,
-    COMPLETE,
-  };
-
-  static void Initialize(
-      Task* task,
-      FunctionPtr start_routine,
-      void* args) {
-    // instantiate a task at the pointer passed in
-    new(task) Task();
-    task->start_routine_ = start_routine;
-    task->args_ = args;
-    task->status_ = Status::QUEUED;
-  }
-
-  void Execute() {
-    args_ = start_routine_(args_);
-    status_ = Status::COMPLETE;
-  }
-
-  void Join() {
-    while (status_ != Status::COMPLETE) {
-      std::this_thread::yield();
-    }
-  }
-
- private:
-  FunctionPtr start_routine_;
-
-  void* args_;
-
-  std::atomic<Status> status_ = Status::QUEUED;
-};
-
-}  // namespace mujoco
-
-#endif  // __cplusplus
-
-#endif  // MUJOCO_SRC_THREAD_TASK_H_

src/thread/thread_pool.cc

@@ -14,39 +14,117 @@
 
 #include "thread/thread_pool.h"
 
+#include <atomic>
 #include <cstddef>
+#include <memory>
+#include <thread>
+#include <utility>
+#include <vector>
 
 #include <mujoco/mjthread.h>
-#include <mujoco/mujoco.h>
+#include "engine/engine_crossplatform.h"
-#include "thread/task.h"
+#include "engine/engine_util_errmem.h"
+#include "thread/thread_queue.h"
+#include "thread/thread_task.h"
 
-static constexpr size_t kMaxThreads = 128;
+namespace mujoco {
+namespace {
+constexpr size_t kThreadPoolQueueSize = 640;
+
+struct WorkerThread {
+  // Shutdown function passed to running threads to ensure clean shutdown.
+  static void* ShutdownFunction(void* args) {
+    return nullptr;
+  }
+
+  // Thread for the worker.
+  std::unique_ptr<std::thread> thread_;
+
+  // An mjTask for shutting down this worker.
+  mjTask shutdown_task_ {
+    &ShutdownFunction,
+    nullptr,
+    mjTASK_NEW
+  };
+};
+}  // namespace
+
+// Concrete C++ class definition for mjThreadPool.
+// (The public mjThreadPool C struct is an opaque one.)
+class ThreadPoolImpl : public mjThreadPool {
+ public:
+  ThreadPoolImpl(int num_worker) : mjThreadPool{num_worker} {
+    // initialize worker threads
+    for (int i = 0; i < num_worker; ++i) {
+      WorkerThread worker{
+        std::make_unique<std::thread>(ThreadPoolWorker, this)};
+      workers_.push_back(std::move(worker));
+    }
+  }
+
+  // start a task in the threadpool
+  void Enqueue(mjTask* task) {
+    if (mjUNLIKELY(GetAtomicTaskStatus(task).exchange(mjTASK_QUEUED) !=
+                   mjTASK_NEW)) {
+      mjERROR("task->status is not mjTASK_NEW");
+    }
+    lockless_queue_.push(task);
+  }
+
+  // shutdown the threadpool
+  void Shutdown() {
+    if (shutdown_) {
+      return;
+    }
+
+    shutdown_ = true;
+    std::vector<mjTask> shutdown_tasks(workers_.size());
+    for (auto& worker : workers_) {
+      Enqueue(&worker.shutdown_task_);
+    }
+
+    for (auto& worker : workers_) {
+      worker.thread_->join();
+    }
+  }
+
+  ~ThreadPoolImpl() { Shutdown(); }
+
+ private:
+  // method executed by running threads
+  static void ThreadPoolWorker(ThreadPoolImpl* thread_pool) {
+    while (!thread_pool->shutdown_) {
+      auto task = static_cast<mjTask*>(thread_pool->lockless_queue_.pop());
+      task->args = task->func(task->args);
+      GetAtomicTaskStatus(task).store(mjTASK_COMPLETED);
+    }
+  }
+
+  // indicates whether the thread pool is being shut down
+  std::atomic<bool> shutdown_ = false;
+
+  // OS threads that are running in this pool
+  std::vector<WorkerThread> workers_;
+
+  // queue of tasks to execute
+  mujoco::LocklessQueue<void*, kThreadPoolQueueSize> lockless_queue_;
+};
 
 // create a thread pool
 mjThreadPool* mju_threadPoolCreate(size_t number_of_threads) {
-  mujoco::ThreadPool<kMaxThreads>* thread_pool =
+  return new ThreadPoolImpl(number_of_threads);
-      new mujoco::ThreadPool<kMaxThreads>(number_of_threads);
-  return static_cast<mjThreadPool*>(static_cast<void*>(thread_pool));
 }
 
 // start a task in the threadpool
-void mju_threadPoolEnqueue(
+void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task) {
-    mjThreadPool* thread_pool, mjTask* task, mjStartRoutine start_routine,
+  auto thread_pool_impl = static_cast<ThreadPoolImpl*>(thread_pool);
-    void* args) {
+  thread_pool_impl->Enqueue(task);
-  mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
-      static_cast<mujoco::ThreadPool<kMaxThreads>*>(
-          static_cast<void*>(thread_pool));
-  thread_pool_ptr->Enqueue(
-      static_cast<mujoco::Task*>(static_cast<void*>(task)), start_routine,
-      args);
 }
 
 // shutdown the threadpool and free the memory
 void mju_threadPoolDestroy(mjThreadPool* thread_pool) {
-  mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
+  auto thread_pool_impl = static_cast<ThreadPoolImpl*>(thread_pool);
-      static_cast<mujoco::ThreadPool<kMaxThreads>*>(
+  thread_pool_impl->Shutdown();
-          static_cast<void*>(thread_pool));
+  delete thread_pool_impl;
-  thread_pool_ptr->Shutdown();
-  delete thread_pool_ptr;
 }
-
+}  // namespace mujoco

src/thread/thread_pool.h

@@ -11,92 +11,32 @@
 // 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.
-// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
-// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
 
 #ifndef MUJOCO_SRC_THREAD_THREAD_POOL_H_
 #define MUJOCO_SRC_THREAD_THREAD_POOL_H_
 
+#include <stddef.h>
+
+#include <mujoco/mjexport.h>
+#include <mujoco/mjthread.h>
+
 #ifdef __cplusplus
-
-#include <atomic>
-#include <cstddef>
-#include <thread>
-
-#include "thread/lockless_queue.h"
-#include "thread/task.h"
-
 namespace mujoco {
+extern "C" {
+#endif
 
-static constexpr size_t kThreadPoolQueueSize = 640;
+// Create a thread pool with the specified number of threads running.
+MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
 
-template <size_t max_number_of_threads>
+// Enqueue a task in a thread pool.
-class ThreadPool {
+MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
- public:
-  ThreadPool(size_t number_of_threads)
-      : number_of_threads_(number_of_threads) {
-    for (int i = 0; i < number_of_threads_; ++i) {
-      threads_[i] = std::thread(ThreadPoolWorker, static_cast<void*>(this));
-    }
-  }
 
-  // start a task in the threadpool
+// Destroy a thread pool.
-  void Enqueue(
+MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool);
-      Task* task, Task::FunctionPtr start_routine, void* args) {
-    Task::Initialize(task, start_routine, args);
-    lockless_queue_.push(static_cast<void*>(task));
-  }
-
-  // shutdown the threadpool
-  void Shutdown() {
-    if (shutdown_) {
-      return;
-    }
-
-    shutdown_ = true;
-    Task shutdown_tasks[max_number_of_threads];
-    for (int i = 0; i < number_of_threads_; ++i) {
-      Enqueue(&shutdown_tasks[i], ShutdownFunction, nullptr);
-    }
-
-    for (int i = 0; i < number_of_threads_; ++i) {
-      threads_[i].join();
-    }
-  }
-
-  ~ThreadPool() { Shutdown(); }
-
- private:
-  // method executed by running threads
-  static void ThreadPoolWorker(void* arg) {
-    ThreadPool<max_number_of_threads>* thread_pool =
-        static_cast<ThreadPool<max_number_of_threads>*>(arg);
-    while (!thread_pool->shutdown_) {
-      Task* task = static_cast<Task*>(thread_pool->lockless_queue_.pop());
-      task->Execute();
-    }
-  }
-
-  // shutdown function passed to running threads to ensure cleans shutdown
-  static void* ShutdownFunction(void* args) {
-    return NULL;
-  }
-
-  // is the thread pool is being shut down
-  std::atomic<bool> shutdown_ = false;
-
-  // actual number of running threads in the threadpool
-  const size_t number_of_threads_;
-
-  // OS threads that are running in this pool
-  std::thread threads_[max_number_of_threads];
-
-  // queue of tasks to execute
-  LocklessQueue<void*, kThreadPoolQueueSize> lockless_queue_;
-};
 
+#ifdef __cplusplus
+}  // extern "C"
 }  // namespace mujoco
-
 #endif  // __cplusplus
 
 #endif  // MUJOCO_SRC_THREAD_THREAD_POOL_H_

src/thread/thread_queue.h

@@ -11,8 +11,6 @@
 // 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.
-// IWYU pragma: private, include "third_party/mujoco/include/mujoco.h"
-// IWYU pragma: friend "third_party/(py/)?mujoco/.*"
 
 #ifndef MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_
 #define MUJOCO_SRC_THREAD_LOCKLESS_QUEUE_H_

src/thread/thread_task.cc

@@ -12,13 +12,22 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "thread/task.h"
+#include "thread/thread_task.h"
+
+#include <thread>
 
 #include <mujoco/mjthread.h>
-#include <mujoco/mujoco.h>
 
-// waits for a task to complete
+namespace mujoco {
-void mju_taskJoin(mjTask* task) {
+void mju_defaultTask(mjTask* task) {
-  mujoco::Task* task_ptr = static_cast<mujoco::Task*>(static_cast<void*>(task));
+  task->func = nullptr;
-  task_ptr->Join();
+  task->args = nullptr;
+  task->status = mjTASK_NEW;
 }
+
+void mju_taskJoin(mjTask* task) {
+  while (GetAtomicTaskStatus(task) != mjTASK_COMPLETED) {
+    std::this_thread::yield();
+  }
+}
+}  // namespace mujoco

src/thread/thread_task.h

@@ -0,0 +1,50 @@
+// 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_SRC_THREAD_THREAD_TASK_H_
+#define MUJOCO_SRC_THREAD_THREAD_TASK_H_
+
+#include <atomic>
+#include <new>
+#include <type_traits>
+
+#include <mujoco/mjexport.h>
+#include <mujoco/mjthread.h>
+
+#ifdef __cplusplus
+namespace mujoco {
+extern "C" {
+#endif
+
+// Initialize an mjTask.
+MJAPI void mju_defaultTask(mjTask* task);
+
+// Wait for a task to complete.
+MJAPI void mju_taskJoin(mjTask* task);
+
+#ifdef __cplusplus
+}  // extern "C"
+
+using TaskStatus = std::remove_volatile_t<decltype(mjTask::status)>;
+inline std::atomic<TaskStatus>& GetAtomicTaskStatus(mjTask* task) {
+  static_assert(sizeof(std::atomic<TaskStatus>) == sizeof(TaskStatus));
+  static_assert(alignof(std::atomic<TaskStatus>) == alignof(TaskStatus));
+  static_assert(std::atomic<TaskStatus>::is_always_lock_free);
+  return *std::launder(reinterpret_cast<std::atomic<TaskStatus>*>(
+      const_cast<TaskStatus*>(&task->status)));
+}
+}  // namespace mujoco
+#endif  // __cplusplus
+
+#endif  // MUJOCO_SRC_THREAD_THREAD_TASK_H_

src/user/user_mesh.cc

@@ -134,6 +134,9 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   mjuu_setvec(pos_volume_, 0, 0, 0);
   mjuu_setvec(quat_surface_, 1, 0, 0, 0);
   mjuu_setvec(quat_volume_, 1, 0, 0, 0);
+  mjuu_setvec(pos_, 0, 0, 0);
+  mjuu_setvec(quat_, 1, 0, 0, 0);
+
   mjuu_setvec(boxsz_surface_, 0, 0, 0);
   mjuu_setvec(boxsz_volume_, 0, 0, 0);
   mjuu_setvec(aabb_, 1e10, 1e10, 1e10);
@@ -391,7 +394,7 @@ void mjCMesh::Compile(const mjVFS* vfs) {
   }
 
   // create using marching cubes
-  if (is_plugin) {
+  else if (is_plugin) {
     LoadSDF();
   }
 
@@ -630,6 +633,18 @@ double* mjCMesh::GetQuatPtr(mjtMeshType type) {
 
 
 
+double* mjCMesh::GetOffsetPosPtr() {
+  return pos_;
+}
+
+
+
+double* mjCMesh::GetOffsetQuatPtr() {
+  return quat_;
+}
+
+
+
 bool mjCMesh::HasTexcoord() const {
   return texcoord_ != nullptr;
 }

src/user/user_model.cc

@@ -1586,6 +1586,7 @@ void mjCModel::CopyTree(mjModel* m) {
       copyvec(m->cam_quat+4*cid, pc->locquat, 4);
       m->cam_fovy[cid] = (mjtNum)pc->fovy;
       m->cam_ipd[cid] = (mjtNum)pc->ipd;
+      copyvec(m->cam_resolution+2*cid, pc->resolution, 2);
       copyvec(m->cam_user+nuser_cam*cid, pc->userdata.data(), nuser_cam);
     }
 
@@ -1737,6 +1738,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
     m->mesh_bvhadr[i] = bvh_adr;
     m->mesh_bvhnum[i] = pme->tree().nbvh;
+    copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
+    copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
 
     // copy vertices, normals, faces, texcoords, aux data
     pme->CopyVert(m->mesh_vert + 3*vert_adr);
@@ -3015,6 +3018,15 @@ bool mjCModel::CopyBack(const mjModel* m) {
     }
   }
 
+  // mesh
+  mjCMesh* pm;
+  for (int i=0; i<nmesh; i++) {
+    pm = meshes[i];
+
+    copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3);
+    copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4);
+  }
+
   // sites
   for (int i=0; i<nsite; i++) {
     copyvec(sites[i]->size, m->site_size + 3 * i, 3);
@@ -3033,6 +3045,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
     copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
     cameras[i]->fovy = (double)m->cam_fovy[i];
     cameras[i]->ipd = (double)m->cam_ipd[i];
+    copyvec(cameras[i]->resolution, m->cam_resolution+2*i, 2);
 
     if (nuser_cam) {
       copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam);

src/user/user_objects.cc

@@ -1747,6 +1747,8 @@ void mjCGeom::Compile(void) {
 
     // apply geom pos/quat as offset
     mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
+    mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3);
+    mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4);
   }
 
   // check size parameters
@@ -1947,6 +1949,7 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
   fovy = 45;
   ipd = 0.068;
   userdata.clear();
+  resolution[0] = resolution[1] = 1;
 
   // clear private variables
   body = 0;
@@ -2001,6 +2004,12 @@ void mjCCamera::Compile(void) {
   if (targetbodyid==body->id) {
     throw mjCError(this, "parent-targeting in camera '%s' (id = %d)", name.c_str(), id);
   }
+
+  // make sure the image size is finite
+  if (fovy >= 180) {
+    throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)",
+                   name.c_str(), id, fovy);
+  }
 }
 
 
@@ -4042,6 +4051,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_TORQUE:
   case mjSENS_MAGNETOMETER:
   case mjSENS_RANGEFINDER:
+  case mjSENS_CAMPROJECTION:
     // must be attached to site
     if (objtype!=mjOBJ_SITE) {
       throw mjCError(this,
@@ -4052,19 +4062,32 @@ void mjCSensor::Compile(void) {
     if (type==mjSENS_TOUCH || type==mjSENS_RANGEFINDER) {
       dim = 1;
       datatype = mjDATATYPE_POSITIVE;
+    } else if (type==mjSENS_CAMPROJECTION) {
+      dim = 2;
+      datatype = mjDATATYPE_REAL;
     } else {
       dim = 3;
       datatype = mjDATATYPE_REAL;
     }
 
     // set stage
-    if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER) {
+    if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER || type==mjSENS_CAMPROJECTION) {
       needstage = mjSTAGE_POS;
     } else if (type==mjSENS_GYRO || type==mjSENS_VELOCIMETER) {
       needstage = mjSTAGE_VEL;
     } else {
       needstage = mjSTAGE_ACC;
     }
+
+    // check for camera resolution for camera projection sensor
+    if (type==mjSENS_CAMPROJECTION) {
+      mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname);
+      if (!camref->resolution[0] || !camref->resolution[1]) {
+        throw mjCError(this,
+                       "camera projection sensor requires camera resolution '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+    }
     break;
 
   case mjSENS_JOINTPOS:

src/user/user_objects.h

@@ -465,6 +465,7 @@ class mjCCamera : public mjCBase {
   double ipd;                     // inter-pupilary distance
   double pos[3];                  // position
   double quat[4];                 // orientation
+  float resolution[2];            // resolution [pixel]
   std::vector<double> userdata;   // user data
   mjCAlternative alt;             // alternative orientation specification
 
@@ -576,6 +577,8 @@ class mjCMesh: public mjCBase {
   void Compile(const mjVFS* vfs);                   // compiler
   double* GetPosPtr(mjtMeshType type);              // get position
   double* GetQuatPtr(mjtMeshType type);             // get orientation
+  double* GetOffsetPosPtr();                        // get position offset for geom
+  double* GetOffsetQuatPtr();                       // get orientation offset for geom
   double* GetInertiaBoxPtr(mjtMeshType type);       // get inertia box
   double& GetVolumeRef(mjtMeshType type);           // get volume
   void FitGeom(mjCGeom* geom, double* meshpos);     // approximate mesh with simple geom
@@ -639,6 +642,8 @@ class mjCMesh: public mjCBase {
   double pos_surface_[3];             // CoM position
   double quat_volume_[4];             // inertia orientation
   double quat_surface_[4];            // inertia orientation
+  double pos_[3];                     // translation applied to asset vertices
+  double quat_[4];                    // rotation applied to asset vertices
   double boxsz_volume_[3];            // half-sizes of equivalent inertia box (volume)
   double boxsz_surface_[3];           // half-sizes of equivalent inertia box (surface)
   double aabb_[6];                    // axis-aligned bounding box

src/user/user_util.cc

@@ -669,6 +669,8 @@ std::string mjuu_extToContentType(std::string_view filename) {
     return "model/stl";
   } else if (!strcasecmp(ext.c_str(), ".obj")) {
     return "model/obj";
+  } else if (!strcasecmp(ext.c_str(), ".ply")) {
+    return "model/ply";
   } else if (!strcasecmp(ext.c_str(), ".msh")) {
     return "model/vnd.mujoco.msh";
   } else if (!strcasecmp(ext.c_str(), ".png")) {

src/xml/xml_native_reader.cc

@@ -80,7 +80,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
 
 //---------------------------------- MJCF schema ---------------------------------------------------
 
-static const int nMJCF = 203;
+static const int nMJCF = 204;
 static const char* MJCF[nMJCF][mjXATTRNUM] = {
 {"mujoco", "!", "1", "model"},
 {"<"},
@@ -151,7 +151,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
             "hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
         {"site", "?", "13", "type", "group", "pos", "quat", "material",
             "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
-        {"camera", "?", "10", "fovy", "ipd", "pos", "quat",
+        {"camera", "?", "11", "fovy", "ipd", "pos", "quat", "resolution",
             "axisangle", "xyaxes", "zaxis", "euler", "mode", "user"},
         {"light", "?", "12", "pos", "dir", "directional", "castshadow", "active",
             "attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"},
@@ -264,7 +264,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
         {">"},
         {"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
             "material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
-        {"camera", "*", "13", "name", "class", "fovy", "ipd",
+        {"camera", "*", "14", "name", "class", "fovy", "ipd", "resolution",
             "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
             "mode", "target", "user"},
         {"light", "*", "15", "name", "class", "directional", "castshadow", "active",
@@ -398,6 +398,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
         {"force", "*", "5", "name", "site", "cutoff", "noise", "user"},
         {"torque", "*", "5", "name", "site", "cutoff", "noise", "user"},
         {"magnetometer", "*", "5", "name", "site", "cutoff", "noise", "user"},
+        {"camprojection", "*", "6", "name", "site", "camera", "cutoff", "noise", "user"},
         {"rangefinder", "*", "5", "name", "site", "cutoff", "noise", "user"},
         {"jointpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
         {"jointvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
@@ -1476,6 +1477,10 @@ void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) {
   ReadAlternative(elem, pcam->alt);
   ReadAttr(elem, "fovy", 1, &pcam->fovy, text);
   ReadAttr(elem, "ipd", 1, &pcam->ipd, text);
+  ReadAttr(elem, "resolution", 2, pcam->resolution, text);
+  if (pcam->resolution[0] < 0 || pcam->resolution[1] < 0) {
+    throw mjXError(elem, "camera resolution cannot be negative");
+  }
 
   // read userdata
   ReadVector(elem, "user", pcam->userdata, text);
@@ -3013,6 +3018,12 @@ void mjXReader::Sensor(XMLElement* section) {
       psen->type = mjSENS_MAGNETOMETER;
       psen->objtype = mjOBJ_SITE;
       ReadAttrTxt(elem, "site", psen->objname, true);
+    } else if (type=="camprojection") {
+      psen->type = mjSENS_CAMPROJECTION;
+      psen->objtype = mjOBJ_SITE;
+      ReadAttrTxt(elem, "site", psen->objname, true);
+      ReadAttrTxt(elem, "camera", psen->refname, true);
+      psen->reftype = mjOBJ_CAMERA;
     } else if (type=="rangefinder") {
       psen->type = mjSENS_RANGEFINDER;
       psen->objtype = mjOBJ_SITE;

src/xml/xml_native_writer.cc

@@ -408,6 +408,7 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) {
   WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->camera.ipd);
   WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->camera.fovy);
   WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->camera.mode);
+  WriteAttr(elem, "resolution", 2, pcam->resolution, def->camera.resolution);
 
   // userdata
   if (writingdefaults) {
@@ -1648,6 +1649,11 @@ void mjXWriter::Sensor(XMLElement* root) {
       elem = InsertEnd(section, "rangefinder");
       WriteAttrTxt(elem, "site", psen->objname);
       break;
+    case mjSENS_CAMPROJECTION:
+      elem = InsertEnd(section, "camprojection");
+      WriteAttrTxt(elem, "site", psen->objname);
+      WriteAttrTxt(elem, "camera", psen->refname);
+      break;
 
     // sensors related to scalar joints, tendons, actuators
     case mjSENS_JOINTPOS:
@@ -1836,7 +1842,7 @@ void mjXWriter::Sensor(XMLElement* root) {
     WriteVector(elem, "user", psen->userdata);
 
     // add reference if present
-    if (psen->reftype != mjOBJ_UNKNOWN) {
+    if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) {
       WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
       WriteAttrTxt(elem, "refname", psen->refname);
     }

test/engine/engine_inverse_test.cc

@@ -104,7 +104,7 @@ TEST_F(InverseTest, DiscreteInverseMatch) {
 
       // depending on mjENBL_INVDISCRETE flag, expect mismatch to be small/large
       if (invdiscrete) {
-        mjtNum epsilon = 1e-10;
+        mjtNum epsilon = 1e-9;
         EXPECT_LT(data->solver_fwdinv[0], epsilon);
         EXPECT_LT(data->solver_fwdinv[1], epsilon);
       } else {

test/engine/engine_plugin_test.cc

@@ -32,7 +32,7 @@ namespace {
 using ::testing::HasSubstr;
 using ::testing::NotNull;
 
-constexpr int kNumTruePlugins = 8;
+constexpr int kNumTruePlugins = 9;
 constexpr int kNumFakePlugins = 30;
 constexpr int kNumTestPlugins = 3;
 

test/engine/engine_sensor_test.cc

@@ -432,5 +432,51 @@ TEST_F(SensorTest, Clock) {
   mj_deleteModel(model);
 }
 
+// ------------------------- camera sensor tests  -----------------------------
+
+// test clock sensor
+TEST_F(SensorTest, CameraProjection) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body pos="1.1 0 1">
+        <geom type="box" size=".1 .6 .375"/>
+        <site name="frontorigin" pos="-.1  .6  .375"/>
+        <site name="frontcorner" pos="-.1 -.6 -.375"/>
+      </body>
+      <body pos="-1.1 0 1">
+        <geom type="box" size=".1 .6 .375"/>
+        <site name="backcenter" pos="-.1 0 0"/>
+      </body>
+      <camera pos="0 0 1" xyaxes="0 -1 0 0 0 1" fovy="41.11209"
+              resolution="1920 1200" name="fixedcamera"/>
+    </worldbody>
+    <sensor>
+      <camprojection site="frontorigin" camera="fixedcamera"/>
+      <camprojection site="frontcorner" camera="fixedcamera"/>
+      <camprojection site="backcenter" camera="fixedcamera"/>
+    </sensor>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+
+  // call step to update sensors
+  mj_step(model, data);
+  mj_step1(model, data);  // update values of position-based sensors
+  EXPECT_THAT(model->cam_resolution[0], 1920);
+  EXPECT_THAT(model->cam_resolution[1], 1200);
+  mjtNum eps = 1e-4;
+  EXPECT_NEAR(data->sensordata[0], 0, eps);
+  EXPECT_NEAR(data->sensordata[1], 0, eps);
+  EXPECT_NEAR(data->sensordata[2], 1920, eps);
+  EXPECT_NEAR(data->sensordata[3], 1200, eps);
+  EXPECT_NEAR(data->sensordata[4], 960, eps);
+  EXPECT_NEAR(data->sensordata[5], 600, eps);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/testdata/sensor/camprojection.xml

@@ -0,0 +1,23 @@
+  <mujoco>
+    <default>
+      <site rgba="1 0 0 1" size="0.03"/>
+    </default>
+    <worldbody>
+      <light pos="0 0 3"/>
+      <body pos="1.1 0 1">
+        <geom type="box" size=".1 .6 .375"/>
+        <site name="frontorigin" pos="-.1  .6  .375"/>
+        <site name="frontcenter" pos="-.1 0 0"/>
+      </body>
+      <body pos="0 0 0">
+        <joint axis="0 0 1" range="-180 180" limited="false"/>
+        <geom type="cylinder" size=".2 .05" pos="0 0 0.9"/>
+        <camera pos="0 0 1" xyaxes="0 -1 0 0 0 1" fovy="41.11209"
+                resolution="1920 1200" name="fixedcamera"/>
+      </body>
+    </worldbody>
+    <sensor>
+      <camprojection site="frontorigin" camera="fixedcamera"/>
+      <camprojection site="frontcenter" camera="fixedcamera"/>
+    </sensor>
+  </mujoco>

test/pipeline_test.cc

@@ -14,6 +14,9 @@
 
 // Tests of the entire pipeline that are not easily associated with one file.
 
+#include <string>
+#include <vector>
+
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <mujoco/mjmodel.h>
@@ -61,5 +64,48 @@ TEST_F(PipelineTest, SparseDenseEquivalent) {
   mj_deleteModel(model);
 }
 
+// mj_forward should be deterministic when warm starts are disabled
+TEST_F(PipelineTest, DeterministicNoWarmstart) {
+  const std::string xml_path = GetTestDataFilePath(kDefaultModel);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  mjData* data = mj_makeData(model);
+  mjData* data2 = mj_makeData(model);
+
+  // disable warmstarts
+  model->opt.disableflags |= mjDSBL_WARMSTART;
+
+  int nv = model->nv;
+
+  int kNumSteps = 50;
+
+  for (mjtSolver solver : {mjSOL_NEWTON, mjSOL_PGS, mjSOL_CG}) {
+    model->opt.solver = solver;
+    mj_resetData(model, data);
+    mj_resetData(model, data2);
+
+    for (int step = 0; step < kNumSteps; step++) {
+      mj_step(model, data);
+      mj_forward(model, data);
+
+      mj_step(model, data2);
+      mj_forward(model, data2);
+
+      // test determinism: both models steps did the same thing
+      EXPECT_EQ(AsVector(data->qacc, nv), AsVector(data2->qacc, nv));
+
+      // one more mj_forward call on data2
+      mj_forward(model, data2);
+
+      // expect that the extra mj_forward call didn't change anything
+      EXPECT_EQ(AsVector(data->qacc, nv), AsVector(data2->qacc, nv));
+    }
+  }
+
+  mj_deleteData(data2);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
 }  // namespace
 }  // namespace mujoco

test/thread/CMakeLists.txt

@@ -0,0 +1,19 @@
+# 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
+#
+#     https://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.
+
+mujoco_test(thread_pool_test)
+target_link_libraries(thread_pool_test fixture gmock)
+
+mujoco_test(thread_queue_test)
+target_link_libraries(thread_queue_test fixture gmock)

test/thread/mjthread_test.cc

@@ -1,90 +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 <mujoco/mjthread.h>
-
-#include <atomic>
-
-#include <gtest/gtest.h>
-#include <mujoco/mujoco.h>
-#include "src/thread/task.h"
-#include "src/thread/thread_pool.h"
-
-namespace {
-
-struct TestFunctionArgs_ {
-  int input;
-  // make this atomic to avoid red-herring tsan failures.
-  std::atomic<int> output;
-};
-typedef struct TestFunctionArgs_ TestFunctionArgs;
-
-void* test_function(void* args) {
-  TestFunctionArgs* test_function_args = static_cast<TestFunctionArgs*>(args);
-  if (!test_function_args) {
-    return nullptr;
-  }
-  test_function_args->output = test_function_args->input;
-  return nullptr;
-}
-
-TEST(TestMjThreadPool, EnsureStructClassSizeMatch) {
-  EXPECT_EQ(sizeof(mjTask), sizeof(mujoco::Task));
-  EXPECT_EQ(sizeof(mjThreadPool), sizeof(mujoco::ThreadPool<128>));
-}
-
-TEST(TestMjThreadPool, TestMjThreadPool10Threads) {
-  mjThreadPool* thread_pool = mju_threadPoolCreate(10);
-
-  TestFunctionArgs test_function_args[1000];
-  mjTask tasks[1000];
-  for (int i = 0; i < 1000; ++i) {
-    test_function_args[i].input = i;
-    mju_threadPoolEnqueue(thread_pool, &tasks[i], test_function,
-                          (void*)&test_function_args[i]);
-  }
-
-  for (int i = 0; i < 1000; ++i) {
-    mju_taskJoin(&tasks[i]);
-  }
-
-  for (int i = 0; i < 1000; ++i) {
-    EXPECT_EQ(test_function_args[i].input, test_function_args[i].output);
-  }
-  mju_threadPoolDestroy(thread_pool);
-}
-
-TEST(TestMjThreadPool, TestMjThreadPool100Threads) {
-  mjThreadPool* thread_pool = mju_threadPoolCreate(100);
-
-  TestFunctionArgs test_function_args[1000];
-  mjTask tasks[1000];
-  for (int i = 0; i < 1000; ++i) {
-    test_function_args[i].input = i;
-    mju_threadPoolEnqueue(thread_pool, &tasks[i], test_function,
-                          (void*)&test_function_args[i]);
-  }
-
-  for (int i = 0; i < 1000; ++i) {
-    mju_taskJoin(&tasks[i]);
-  }
-
-  for (int i = 0; i < 1000; ++i) {
-    EXPECT_EQ(test_function_args[i].input, test_function_args[i].output);
-  }
-
-  mju_threadPoolDestroy(thread_pool);
-}
-
-}  // namespace

test/thread/task_test.cc

@@ -1,46 +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 "src/thread/task.h"
-
-#include <gtest/gtest.h>
-
-namespace mujoco {
-namespace {
-
-struct TestFunctionArgs {
-  int input;
-  int output;
-};
-
-void* test_function(void* args) {
-  TestFunctionArgs* test_function_args = (TestFunctionArgs*)args;
-  test_function_args->output = test_function_args->input;
-  return nullptr;
-}
-
-TEST(TestMjThread, TestMjThread) {
-  TestFunctionArgs test_function_args;
-  test_function_args.input = 1;
-  test_function_args.output = 2;
-  Task task;
-  Task::Initialize(
-      &task, test_function, static_cast<void*>(&test_function_args));
-  task.Execute();
-  task.Join();
-  EXPECT_EQ(test_function_args.input, test_function_args.output);
-}
-
-}  // namespace
-}  // namespace mujoco

test/thread/thread_pool_test.cc

@@ -12,8 +12,6 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "src/thread/thread_pool.h"
-
 #include <atomic>
 #include <condition_variable>
 #include <memory>
@@ -21,75 +19,82 @@
 #include <thread>
 
 #include <gtest/gtest.h>
-#include "src/thread/task.h"
+#include <mujoco/mujoco.h>
 
 namespace mujoco {
 namespace {
 
-struct TestFunctionArgs {
+struct TestFunctionArgs_ {
   int input;
   // make this atomic to avoid red-herring tsan failures.
   std::atomic<int> output;
 };
+typedef struct TestFunctionArgs_ TestFunctionArgs;
 
 void* test_function(void* args) {
   TestFunctionArgs* test_function_args = static_cast<TestFunctionArgs*>(args);
+  if (!test_function_args) {
+    return nullptr;
+  }
   test_function_args->output = test_function_args->input;
   return nullptr;
 }
 
 TEST(TestMjThreadPool, TestMjThreadPool10Threads) {
-  ThreadPool<10> thread_pool(10);
+  mjThreadPool* thread_pool = mju_threadPoolCreate(10);
 
   constexpr int kTasks = 1000;
   TestFunctionArgs test_function_args[kTasks];
-  Task tasks[kTasks];
+  mjTask tasks[kTasks];
   for (int i = 0; i < kTasks; ++i) {
     test_function_args[i].input = i;
-    thread_pool.Enqueue(
+    mju_defaultTask(&tasks[i]);
-        &tasks[i], test_function, static_cast<void*>(&test_function_args[i]));
+    tasks[i].func = test_function;
+    tasks[i].args = &test_function_args[i];
+    mju_threadPoolEnqueue(thread_pool, &tasks[i]);
   }
 
   for (int i = 0; i < kTasks; ++i) {
-    tasks[i].Join();
+    mju_taskJoin(&tasks[i]);
   }
 
   for (int i = 0; i < kTasks; ++i) {
     EXPECT_EQ(test_function_args[i].input, test_function_args[i].output);
   }
-
+  mju_threadPoolDestroy(thread_pool);
-  thread_pool.Shutdown();
 }
 
 TEST(TestMjThreadPool, TestMjThreadPool100Threads) {
-  ThreadPool<100> thread_pool(100);
+  mjThreadPool* thread_pool = mju_threadPoolCreate(100);
 
   constexpr int kTasks = 1000;
   TestFunctionArgs test_function_args[kTasks];
-  Task tasks[kTasks];
+  mjTask tasks[kTasks];
   for (int i = 0; i < kTasks; ++i) {
     test_function_args[i].input = i;
-    thread_pool.Enqueue(
+    mju_defaultTask(&tasks[i]);
-        &tasks[i], test_function, static_cast<void*>(&test_function_args[i]));
+    tasks[i].func = test_function;
+    tasks[i].args = &test_function_args[i];
+    mju_threadPoolEnqueue(thread_pool, &tasks[i]);
   }
 
   for (int i = 0; i < kTasks; ++i) {
-    tasks[i].Join();
+    mju_taskJoin(&tasks[i]);
   }
 
   for (int i = 0; i < kTasks; ++i) {
     EXPECT_EQ(test_function_args[i].input, test_function_args[i].output);
   }
 
-  thread_pool.Shutdown();
+  mju_threadPoolDestroy(thread_pool);
 }
 
 TEST(TestMjThreadPool, TestMjThreadPoolManyWriters) {
-  ThreadPool<10> thread_pool(10);
+  mjThreadPool* thread_pool = mju_threadPoolCreate(10);
 
   constexpr int kTasks = 20;
   TestFunctionArgs test_function_args[kTasks];
-  Task tasks[kTasks];
+  mjTask tasks[kTasks];
   std::unique_ptr<std::thread> enqueue_threads[kTasks];
 
   // add tasks to the thread pool from many threads
@@ -98,6 +103,10 @@ TEST(TestMjThreadPool, TestMjThreadPoolManyWriters) {
   bool start = false;
   for (int i = 0; i < kTasks; ++i) {
     test_function_args[i].input = i;
+    mju_defaultTask(&tasks[i]);
+    tasks[i].func = &test_function;
+    tasks[i].args = &test_function_args[i];
+
     enqueue_threads[i] = std::make_unique<std::thread>([&, i] {
       // synchronize all threads adding to the thread_pool at the same time
       {
@@ -105,8 +114,7 @@ TEST(TestMjThreadPool, TestMjThreadPoolManyWriters) {
         start_cv.wait(lock, [&] { return start; });
       }
       // enqueue outside the lock, to get some concurrency
-      thread_pool.Enqueue(
+      mju_threadPoolEnqueue(thread_pool, &tasks[i]);
-          &tasks[i], test_function, static_cast<void*>(&test_function_args[i]));
     });
   }
   {
@@ -120,14 +128,14 @@ TEST(TestMjThreadPool, TestMjThreadPoolManyWriters) {
   }
 
   for (int i = 0; i < kTasks; ++i) {
-    tasks[i].Join();
+    mju_taskJoin(&tasks[i]);
   }
 
   for (int i = 0; i < kTasks; ++i) {
     EXPECT_EQ(test_function_args[i].input, test_function_args[i].output);
   }
 
-  thread_pool.Shutdown();
+  mju_threadPoolDestroy(thread_pool);
 }
 
 }  // namespace

test/thread/thread_queue_test.cc

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "src/thread/lockless_queue.h"
+#include "src/thread/thread_queue.h"
 
 #include <cstddef>
 

test/user/user_mesh_test.cc

@@ -802,5 +802,62 @@ TEST_F(MjCMeshTest, ExactShellInertia) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjCMeshTest, MeshPosQuat) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <asset>
+      <mesh name="pyramid" vertex="0 0 0  1 0 0  0 1 0  0 0 1"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" name="geom1" mesh="pyramid"/>
+      <geom type="mesh" name="geom2" pos="1 2 3" quat="0.5 0.5 0.5 0.5" mesh="pyramid"/>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, testing::NotNull());
+  // Loading the mesh results in an offset of the geom's pos and quat due to the
+  // fact that the geom's center is not the volumetric center of the mesh. To
+  // recover the geom's originally specified pose, the offset used is stored in
+  // mesh_pos and mesh_quat. In order to recover the originally specified pose
+  // and orientation, first invert the specified mesh_pos and mesh_quat.
+  mjtNum inverse_mesh_pos[3];
+  mjtNum inverse_mesh_quat[4];
+  mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
+              &model->mesh_pos[0], &model->mesh_quat[0]);
+
+  // Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
+  // quat. It should match the originally specified values.
+  double recovered_pos[3];
+  double recovered_quat[4];
+  mju_mulPose(recovered_pos, recovered_quat,
+              &model->geom_pos[0], &model->geom_quat[0],
+              inverse_mesh_pos, inverse_mesh_quat);
+  EXPECT_NEAR(recovered_pos[0], 0, 1e-12);
+  EXPECT_NEAR(recovered_pos[1], 0, 1e-12);
+  EXPECT_NEAR(recovered_pos[2], 0, 1e-12);
+
+  EXPECT_NEAR(recovered_quat[0], 1, 1e-12);
+  EXPECT_NEAR(recovered_quat[1], 0, 1e-12);
+  EXPECT_NEAR(recovered_quat[2], 0, 1e-12);
+  EXPECT_NEAR(recovered_quat[3], 0, 1e-12);
+
+  // Same test on the other geom.
+  mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
+              &model->mesh_pos[0], &model->mesh_quat[0]);
+  mju_mulPose(recovered_pos, recovered_quat,
+              &model->geom_pos[3], &model->geom_quat[4],
+              inverse_mesh_pos, inverse_mesh_quat);
+  EXPECT_NEAR(recovered_pos[0], 1, 1e-12);
+  EXPECT_NEAR(recovered_pos[1], 2, 1e-12);
+  EXPECT_NEAR(recovered_pos[2], 3, 1e-12);
+
+  EXPECT_NEAR(recovered_quat[0], 0.5, 1e-12);
+  EXPECT_NEAR(recovered_quat[1], 0.5, 1e-12);
+  EXPECT_NEAR(recovered_quat[2], 0.5, 1e-12);
+  EXPECT_NEAR(recovered_quat[3], 0.5, 1e-12);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco

test/user/user_objects_test.cc

@@ -732,6 +732,28 @@ TEST_F(QuatNorm, QuatNotNormalized) {
   mj_deleteModel(m);
 }
 
+// ------------- test camera specifications ------------------------------------
+
+using CameraSpecTest = MujocoTest;
+
+TEST_F(CameraSpecTest, FovyLimits) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <camera fovy="180"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(m, IsNull()) << error.data();
+  EXPECT_THAT(error.data(), HasSubstr("fovy too large"));
+  mj_deleteModel(m);
+}
+
 // ------------- test actuator order -------------------------------------------
 
 using ActuatorTest = MujocoTest;

test/xml/xml_native_writer_test.cc

@@ -1063,7 +1063,8 @@ TEST_F(PluginTest, WriteReadCompare) {
 
         // if file is meant to fail, skip it
         if (absl::StrContains(p.path().string(), "malformed_") ||
-            absl::StrContains(p.path().string(), "touch_grid")) {
+            absl::StrContains(p.path().string(), "touch_grid") ||
+            absl::StrContains(p.path().string(), "cow")) {
           continue;
         }