chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Abhishek Joshi
2023-09-14 17:29:15 -05:00
committed by GitHub
parent 8b9ffdc441 2aea61942c
commit ea0aa86e6b
73 changed files with 17267 additions and 771 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/MujocoDependencies.cmake
+26
View File
@@ -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
+544
View File
@@ -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
+9 -3
View File
@@ -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
- The size of the preallocated array ``mjData.solver``. This is used to store diagnostic information about each
iteration of the constraint solver. The actual number of iterations is given by ``mjData.solver_iter``.
- 200
- The number of iterations where 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 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
+21 -12
View File
@@ -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.
.. _mju_taskJoin:
mju_taskJoin
~~~~~~~~~~~~
.. mujoco-include:: mju_taskJoin
Waits for a task to complete.
Enqueue a task in a thread pool.
.. _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
+54 -12
View File
@@ -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
orientation; see also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot
models are designed in a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we
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.
Below is an MJCF model fragment of a forearm, containing all the information needed to put the mesh where one would
expect it to be. The body position is specified relative to the parent body, namely the upper arm (not shown). It is
offset by 35 cm which is the typical length of the human upper arm. If the mesh vertex data were not designed in the
above convention, we would have to use the geom position and orientation (or the new refpos, refquat mechanism) to
compensate, but in practice this is rarely needed.
rotation offsets needed to achieve such alignment in :ref:`mjModel.mesh_pos<mjModel>` and
:ref:`mjModel.mesh_quat<mjModel>`. These offsets are then applied to the referencing geom's position and orientation; see
also :at:`mesh` attribute of :ref:`geom <body-geom>` below. Fortunately most meshes used in robot models are designed in
a coordinate frame centered at the joint. This makes the corresponding MJCF model intuitive: we 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. Below is an MJCF model
fragment of a forearm, containing all the information needed to put the mesh where one would expect it to be. The body
position is specified relative to the parent body, namely the upper arm (not shown). It is offset by 35 cm which is the
typical length of the human upper arm. If the mesh vertex data were not designed in the above convention, we would have
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
+14 -5
View File
@@ -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
+27 -12
View File
@@ -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
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
10. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
10. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
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
used to determine the type of the asset file without resorting to pulling the type from the file extension.
`Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`__ (previously known as MIME type)
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
+23 -12
View File
@@ -146,9 +146,10 @@ struct mjData_ {
mjTimerStat timer[mjNTIMER]; // timer statistics
// solver statistics
mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration
int solver_iter; // number of solver iterations
int solver_nnz; // number of non-zeros in Hessian or efc_AR
mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration
int solver_nisland; // number of islands processed by solver
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_ {
char buffer[24];
};
typedef struct mjTask_ mjTask;
typedef enum mjtTaskStatus_ { // status values for mjTask
mjTASK_NEW = 0, // newly created
mjTASK_QUEUED, // enqueued in a thread pool
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(
ThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
id* args);
void mju_taskJoin(mjTask* task);
void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
void mju_threadPoolDestroy(mjThreadPool* thread_pool);
void mju_defaultTask(mjTask* task);
void mju_taskJoin(mjTask* task);
// NOLINTEND
doc/models.rst
+46 -2
View File
@@ -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
+1 -1
View File
@@ -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
+4 -3
View File
@@ -173,9 +173,10 @@ struct mjData_ {
mjTimerStat timer[mjNTIMER]; // timer statistics
// solver statistics
mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration
int solver_iter; // number of solver iterations
int solver_nnz; // number of non-zeros in Hessian or efc_AR
mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration
int solver_nisland; // number of islands processed by solver
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
+6 -1
View File
@@ -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 -22
View File
@@ -15,33 +15,26 @@
#ifndef MUJOCO_INCLUDE_MJTHREAD_H_
#define MUJOCO_INCLUDE_MJTHREAD_H_
// C API for MuJoCo threading
#ifdef __cplusplus
extern "C" {
#endif
typedef enum mjtTaskStatus_ { // status values for mjTask
mjTASK_NEW = 0, // newly created
mjTASK_QUEUED, // enqueued in a thread pool
mjTASK_COMPLETED // completed execution
} mjtTaskStatus;
#include <stddef.h>
#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;
// function pointer type for mjTask
typedef void* (*mjfTask)(void*);
// 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*);
typedef mjStartRoutine_ mjStartRoutine;
#ifdef __cplusplus
}
#endif
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;
#endif // MUJOCO_INCLUDE_MJTHREAD_H_
include/mujoco/mjxmacro.h
+12 -8
View File
@@ -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_nnz ) \
X( int, solver_nisland ) \
X( int, nbodypair_broad ) \
X( int, nbodypair_narrow ) \
X( int, ngeompair_mid ) \
@@ -651,12 +653,14 @@
// vector fields of mjData
#define MJDATA_VECTOR \
X( mjWarningStat, warning, mjNWARNING, 1 ) \
X( mjTimerStat, timer, mjNTIMER, 1 ) \
X( mjSolverStat, solver, mjNSOLVER, 1 ) \
X( mjtNum, solver_fwdinv, 2, 1 ) \
X( mjtNum, energy, 2, 1 )
#define MJDATA_VECTOR \
X( mjWarningStat, warning, mjNWARNING, 1 ) \
X( mjTimerStat, timer, mjNTIMER, 1 ) \
X( mjSolverStat, solver, mjNILSAND, mjNSOLVER ) \
X( int, solver_niter, mjNISLAND, 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
+10 -8
View File
@@ -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.
MJAPI void mju_threadPoolEnqueue(
mjThreadPool* thread_pool, mjTask* task, void*(start_routine)(void*),
void* args);
// Enqueue a task in a thread pool.
MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
// 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
+4 -1
View File
@@ -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
+42 -31
View File
@@ -313,37 +313,38 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjSENS_TORQUE', 5),
('mjSENS_MAGNETOMETER', 6),
('mjSENS_RANGEFINDER', 7),
('mjSENS_JOINTPOS', 8),
('mjSENS_JOINTVEL', 9),
('mjSENS_TENDONPOS', 10),
('mjSENS_TENDONVEL', 11),
('mjSENS_ACTUATORPOS', 12),
('mjSENS_ACTUATORVEL', 13),
('mjSENS_ACTUATORFRC', 14),
('mjSENS_JOINTACTFRC', 15),
('mjSENS_BALLQUAT', 16),
('mjSENS_BALLANGVEL', 17),
('mjSENS_JOINTLIMITPOS', 18),
('mjSENS_JOINTLIMITVEL', 19),
('mjSENS_JOINTLIMITFRC', 20),
('mjSENS_TENDONLIMITPOS', 21),
('mjSENS_TENDONLIMITVEL', 22),
('mjSENS_TENDONLIMITFRC', 23),
('mjSENS_FRAMEPOS', 24),
('mjSENS_FRAMEQUAT', 25),
('mjSENS_FRAMEXAXIS', 26),
('mjSENS_FRAMEYAXIS', 27),
('mjSENS_FRAMEZAXIS', 28),
('mjSENS_FRAMELINVEL', 29),
('mjSENS_FRAMEANGVEL', 30),
('mjSENS_FRAMELINACC', 31),
('mjSENS_FRAMEANGACC', 32),
('mjSENS_SUBTREECOM', 33),
('mjSENS_SUBTREELINVEL', 34),
('mjSENS_SUBTREEANGMOM', 35),
('mjSENS_CLOCK', 36),
('mjSENS_PLUGIN', 37),
('mjSENS_USER', 38),
('mjSENS_CAMPROJECTION', 8),
('mjSENS_JOINTPOS', 9),
('mjSENS_JOINTVEL', 10),
('mjSENS_TENDONPOS', 11),
('mjSENS_TENDONVEL', 12),
('mjSENS_ACTUATORPOS', 13),
('mjSENS_ACTUATORVEL', 14),
('mjSENS_ACTUATORFRC', 15),
('mjSENS_JOINTACTFRC', 16),
('mjSENS_BALLQUAT', 17),
('mjSENS_BALLANGVEL', 18),
('mjSENS_JOINTLIMITPOS', 19),
('mjSENS_JOINTLIMITVEL', 20),
('mjSENS_JOINTLIMITFRC', 21),
('mjSENS_TENDONLIMITPOS', 22),
('mjSENS_TENDONLIMITVEL', 23),
('mjSENS_TENDONLIMITFRC', 24),
('mjSENS_FRAMEPOS', 25),
('mjSENS_FRAMEQUAT', 26),
('mjSENS_FRAMEXAXIS', 27),
('mjSENS_FRAMEYAXIS', 28),
('mjSENS_FRAMEZAXIS', 29),
('mjSENS_FRAMELINVEL', 30),
('mjSENS_FRAMEANGVEL', 31),
('mjSENS_FRAMELINACC', 32),
('mjSENS_FRAMEANGACC', 33),
('mjSENS_SUBTREECOM', 34),
('mjSENS_SUBTREELINVEL', 35),
('mjSENS_SUBTREEANGMOM', 36),
('mjSENS_CLOCK', 37),
('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
+31 -27
View File
@@ -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.',
)),
('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.',
doc='Enqueue a task in a thread pool.',
)),
('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
+64 -26
View File
@@ -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'),
doc='number of non-zeros in Hessian or efc_AR',
type=ArrayType(
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',
type=ArrayType(
inner_type=ValueType(name='char'),
extents=(6208,),
name='nworker',
type=ValueType(name='int'),
doc='number of workers in the pool',
),
),
)),
('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
+2
View File
@@ -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/asset/spot.obj
+12011
View File
File diff suppressed because it is too large Load Diff
model/plugin/sdf/asset/spot.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 77 KiB

model/plugin/sdf/cow.xml
+64
View File
@@ -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
+3 -1
View File
@@ -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
+2
View File
@@ -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
+161
View File
@@ -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
+54
View File
@@ -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
+2 -1
View File
@@ -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
+2
View File
@@ -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
+2 -1
View File
@@ -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
+118 -55
View File
@@ -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
sim->figconstraint.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (int i=1; i<5; i++) {
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;
// reset lines in Constraint and Cost figures
memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int));
memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int));
// y
sim->figconstraint.linedata[0][2*i+1] = d->nefc;
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;
}
// number of islands that have diagnostics
int nisland = mjMIN(d->solver_nisland, mjNISLAND);
// update cost figure
sim->figcost.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (int i=1; i<3; i++) {
sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
}
if (m->opt.solver==mjSOL_PGS) {
sim->figcost.linepnt[1] = 0;
sim->figcost.linepnt[2] = 0;
}
// iterate over islands
for (int k=0; k < nisland; k++) {
// ==== update Constraint ("Counts") figure
for (int i=0; i<sim->figcost.linepnt[0]; i++) {
// x
sim->figcost.linedata[0][2*i] = i;
sim->figcost.linedata[1][2*i] = i;
sim->figcost.linedata[2][2*i] = i;
// number of points to plot, starting line
int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT);
int start = kConstraintNum * k;
// y
sim->figcost.linedata[0][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].improvement));
sim->figcost.linedata[1][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].gradient));
sim->figcost.linedata[2][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].lineslope));
sim->figconstraint.linepnt[start + 0] = npoints;
for (int i=1; i < kConstraintNum; i++) {
sim->figconstraint.linepnt[start + i] = npoints;
}
if (m->opt.solver == mjSOL_PGS) {
sim->figconstraint.linepnt[start + 3] = 0;
sim->figconstraint.linepnt[start + 4] = 0;
}
if (m->opt.solver == mjSOL_CG) {
sim->figconstraint.linepnt[start + 4] = 0;
}
for (int i=0; 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) {
int ind = mjMIN(d->solver_iter-1, mjNSOLVER-1);
solerr = mju_min(d->solver[ind].improvement, d->solver[ind].gradient);
if (solerr==0) {
solerr = mju_max(d->solver[ind].improvement, d->solver[ind].gradient);
for (int i=0; i < nisland; i++) {
mjtNum solerr_i = 0;
if (d->solver_niter[i]) {
int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1;
const mjSolverStat* stat = d->solver + i*mjNSOLVER + ind;
solerr_i = mju_min(stat->improvement, stat->gradient);
if (solerr_i==0) {
solerr_i = mju_max(stat->improvement, stat->gradient);
}
}
solerr = mju_max(solerr, solerr_i);
}
solerr = mju_log10(mju_max(mjMINVAL, solerr));
@@ -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_sensor = this->sensor && (this->run || !this->m_);
bool update_profiler = this->profiler && (this->pause_update || this->run);
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
+4 -2
View File
@@ -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
+1 -1
View File
@@ -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
+5 -3
View File
@@ -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* quat = m->geom_quat + 4 * g;
mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[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
+8 -2
View File
@@ -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
-41
View File
@@ -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
+99
View File
@@ -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
+12
View File
@@ -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
+7 -4
View File
@@ -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
+10 -18
View File
@@ -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));
d->solver_iter = 0;
d->solver_nnz = 0;
memset(d->solver, 0, mjNSOLVER*mjNISLAND*sizeof(mjSolverStat));
d->solver_nisland = 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
+25 -16
View File
@@ -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_nnz = %d\n", d->solver_nnz);
for (int i=0; i < mjMIN(mjNSOLVER, d->solver_iter); i++) {
fprintf(fp, " %d: improvement = ", i);
fprintf(fp, float_format, d->solver[i].improvement);
fprintf(fp, " gradient = ");
fprintf(fp, float_format, d->solver[i].gradient);
fprintf(fp, " lineslope = ");
fprintf(fp, float_format, d->solver[i].lineslope);
fprintf(fp, "\n");
fprintf(fp, " nactive = %d nchange = %d neval = %d nupdate = %d\n",
d->solver[i].nactive, d->solver[i].nchange,
d->solver[i].neval, d->solver[i].nupdate);
fprintf(fp, " solver_nisland = %d\n", d->solver_nisland);
printVector(" solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
int nisland_stat = mjMIN(d->solver_nisland, mjNISLAND);
for (int island=0; island < nisland_stat; island++) {
int niter_stat = mjMIN(mjNSOLVER, d->solver_niter[island]);
if (niter_stat) {
fprintf(fp, " ISLAND %d\n", island);
fprintf(fp, " solver_niter = %d\n", d->solver_niter[island]);
fprintf(fp, " solver_nnz = %d\n", d->solver_nnz[island]);
for (int i=0; i < niter_stat; i++) {
mjSolverStat* stat = d->solver + island*mjNSOLVER + i;
fprintf(fp, " %d: improvement = ", i);
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
+3
View File
@@ -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
+90
View File
@@ -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
+71 -40
View File
@@ -39,24 +39,30 @@ static mjtNum rescale(const mjModel* m, mjtNum x) {
// save solver statistics, count
static void saveStats(const mjModel* m, mjData* d, int* piter,
// save solver statistics
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
int i = d->solver_iter + (*piter);
(*piter)++;
// 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 out of range, return
if (island >= mjNISLAND) {
return;
}
// 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
d->solver_iter += iter;
// finalize statistics
if (island < mjNISLAND) {
// update solver iterations
d->solver_niter[island] += iter;
// set nnz
if (mj_isSparse(m)) {
d->solver_nnz = 0;
for (int i=0; i < nefc; i++) {
d->solver_nnz += d->efc_AR_rownnz[i];
// set nnz
if (mj_isSparse(m)) {
d->solver_nnz[island] = 0;
for (int i=0; i < nefc; 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
d->solver_iter += iter;
// finalize statistics
if (island < mjNISLAND) {
// update solver iterations
d->solver_niter[island] += iter;
// set solver_nnz
if (flg_Newton) {
if (mj_isSparse(m)) {
d->solver_nnz = 2*ctx.nnz - nv;
// set solver_nnz
if (flg_Newton) {
if (mj_isSparse(m)) {
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
+1 -1
View File
@@ -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
+3 -3
View File
@@ -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
-70
View File
@@ -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
+99 -21
View File
@@ -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 "thread/task.h"
#include "engine/engine_crossplatform.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 =
new mujoco::ThreadPool<kMaxThreads>(number_of_threads);
return static_cast<mjThreadPool*>(static_cast<void*>(thread_pool));
return new ThreadPoolImpl(number_of_threads);
}
// start a task in the threadpool
void mju_threadPoolEnqueue(
mjThreadPool* thread_pool, mjTask* task, mjStartRoutine start_routine,
void* args) {
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);
void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task) {
auto thread_pool_impl = static_cast<ThreadPoolImpl*>(thread_pool);
thread_pool_impl->Enqueue(task);
}
// shutdown the threadpool and free the memory
void mju_threadPoolDestroy(mjThreadPool* thread_pool) {
mujoco::ThreadPool<kMaxThreads>* thread_pool_ptr =
static_cast<mujoco::ThreadPool<kMaxThreads>*>(
static_cast<void*>(thread_pool));
thread_pool_ptr->Shutdown();
delete thread_pool_ptr;
auto thread_pool_impl = static_cast<ThreadPoolImpl*>(thread_pool);
thread_pool_impl->Shutdown();
delete thread_pool_impl;
}
} // namespace mujoco
src/thread/thread_pool.h
+15 -75
View File
@@ -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>
class ThreadPool {
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));
}
}
// Enqueue a task in a thread pool.
MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
// start a task in the threadpool
void Enqueue(
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_;
};
// Destroy a thread pool.
MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool);
#ifdef __cplusplus
} // extern "C"
} // namespace mujoco
#endif // __cplusplus
#endif // MUJOCO_SRC_THREAD_THREAD_POOL_H_
src/thread/lockless_queue.h → src/thread/thread_queue.h
-2
View File
@@ -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/task.cc → src/thread/thread_task.cc
+15 -6
View File
@@ -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
void mju_taskJoin(mjTask* task) {
mujoco::Task* task_ptr = static_cast<mujoco::Task*>(static_cast<void*>(task));
task_ptr->Join();
namespace mujoco {
void mju_defaultTask(mjTask* task) {
task->func = nullptr;
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
+50
View File
@@ -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
+16 -1
View File
@@ -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
+13
View File
@@ -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
+24 -1
View File
@@ -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
+5
View File
@@ -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
+2
View File
@@ -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
+14 -3
View File
@@ -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
+7 -1
View File
@@ -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
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+46
View File
@@ -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
Vendored
+23
View File
@@ -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
+46
View File
@@ -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
+19
View File
@@ -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
-90
View File
@@ -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
-46
View File
@@ -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
+31 -23
View File
@@ -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(
&tasks[i], test_function, static_cast<void*>(&test_function_args[i]));
mju_defaultTask(&tasks[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, 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(
&tasks[i], test_function, static_cast<void*>(&test_function_args[i]));
mju_defaultTask(&tasks[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(
&tasks[i], test_function, static_cast<void*>(&test_function_args[i]));
mju_threadPoolEnqueue(thread_pool, &tasks[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/lockless_queue_test.cc → test/thread/thread_queue_test.cc
+1 -1
View File
@@ -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
+57
View File
@@ -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
+22
View File
@@ -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
+2 -1
View File
@@ -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;
}
unity/Runtime/Bindings/MjBindings.cs
+3045 -53
View File
File diff suppressed because it is too large Load Diff