diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index a3438f5c..52c5df88 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -19,12 +19,12 @@ jobs: - name: "Update dependencies" run: sudo apt-get update - name: "Install dependencies" - run: sudo apt-get install -y build-essential cmake pkg-config qtbase5-dev libqt5opengl5-dev libgl1-mesa-dev libglu1-mesa-dev libprotobuf-dev protobuf-compiler libode-dev libboost-dev xvfb libgl1-mesa-dri + run: sudo apt-get install -y build-essential cmake pkg-config qtbase5-dev libqt5opengl5-dev libgl1-mesa-dev libglu1-mesa-dev libprotobuf-dev protobuf-compiler libode-dev libboost-dev libopenblas-dev xvfb libgl1-mesa-dri - name: "Build" run: make - name: "Build regression tests" run: | - cmake -S . -B build -DBUILD_TESTING=ON + cmake -S . -B build -DBUILD_TESTING=ON -DBUILD_ODE=ON cmake --build build -j 2 - name: "Run regression tests" working-directory: build diff --git a/.gitignore b/.gitignore index 2f15451b..33ed0ef2 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,6 @@ Thumbs.db *.iml cmake-build-debug cmake-build-release + +# Local experiment reports, raw data and profiles. +/docs/research/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ede6a8c..8467bf38 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,22 +91,30 @@ find_package(Qt5 COMPONENTS Core Widgets OpenGL Network REQUIRED) list(APPEND libs Qt5::Core Qt5::Widgets Qt5::OpenGL Qt5::Network) # ODE +find_package(Threads REQUIRED) +list(APPEND libs Threads::Threads) +option(BUILD_ODE "Build the bundled double-precision ODE dependency." OFF) +set(GRSIM_ODE_MIN_VERSION 0.16.6) if(BUILD_ODE) include(BuildODE) add_dependencies(${app} ode_external) else() if(WIN32) - find_package(ODE CONFIG) + find_package(ODE ${GRSIM_ODE_MIN_VERSION} QUIET CONFIG) + if(ODE_FOUND AND NOT ODE_DOUBLE_PRECISION) + set(ODE_FOUND FALSE) + endif() set(ODE_LIB_NAME ODE::ODE) else() - find_package(ODE) + find_package(ODE ${GRSIM_ODE_MIN_VERSION} QUIET) set(ODE_LIB_NAME ode::ode) endif() if(ODE_FOUND) + message(STATUS "Using system ODE ${ODE_VERSION}") list(APPEND libs ${ODE_LIB_NAME}) else() - # if ODE could not be found just build it + # Older and single-precision system libraries cannot satisfy this build. include(BuildODE) add_dependencies(${app} ode_external) endif() @@ -201,6 +209,7 @@ set(SOURCES src/physics/pfixedbox.cpp src/physics/pcylinder.cpp src/physics/pchassiscollision.cpp + src/physics/pcollisionsphere.cpp src/physics/pbox.cpp src/physics/pray.cpp src/net/robocup_ssl_server.cpp @@ -227,6 +236,7 @@ set(HEADERS include/physics/pfixedbox.h include/physics/pcylinder.h include/physics/pchassiscollision.h + include/physics/pcollisionsphere.h include/physics/pbox.h include/physics/pray.h include/net/robocup_ssl_server.h @@ -348,6 +358,85 @@ if(BUILD_TESTING) add_test(NAME geometry_regression COMMAND grSim_geometry_test) set_tests_properties(geometry_regression PROPERTIES ENVIRONMENT "QT_QPA_PLATFORM=offscreen" TIMEOUT 15) + add_test(NAME worker_ode_policy_regression COMMAND grSim_worker_test --ode-worker-policy) + set_tests_properties(worker_ode_policy_regression PROPERTIES + ENVIRONMENT "QT_QPA_PLATFORM=offscreen" TIMEOUT 30) + add_test(NAME worker_regression_ode_threads COMMAND grSim_worker_test) + set_tests_properties(worker_regression_ode_threads PROPERTIES + ENVIRONMENT "QT_QPA_PLATFORM=offscreen;GRSIM_ODE_THREADS=2" TIMEOUT 30) + if(TARGET ode_external) + set(kernel_test_dir "${CMAKE_CURRENT_BINARY_DIR}/ode-kernel-tests") + set(kernel_test_headers "") + foreach(variant reference optimized) + foreach(kernel fastlsolve_impl.h fastltsolve_impl.h fastdot_impl.h) + list(APPEND kernel_test_headers "${kernel_test_dir}/${variant}_${kernel}") + endforeach() + endforeach() + add_custom_command(OUTPUT ${kernel_test_headers} + COMMAND "${CMAKE_COMMAND}" + "-DSOURCE_DIR=${GRSIM_BUNDLED_ODE_SOURCE_DIR}" + "-DOUTPUT_DIR=${kernel_test_dir}" "-DGIT_EXECUTABLE=${GIT_EXECUTABLE}" + -P "${PROJECT_SOURCE_DIR}/cmake/ode/PrepareKernelTests.cmake" + DEPENDS ode_external "${PROJECT_SOURCE_DIR}/cmake/ode/PrepareKernelTests.cmake" + "${PROJECT_SOURCE_DIR}/cmake/ode/cpu-math.patch" + "${PROJECT_SOURCE_DIR}/cmake/ode/grsim_ode_math.h") + add_custom_target(grSim_ode_kernel_headers DEPENDS ${kernel_test_headers}) + add_executable(grSim_ode_math_fallback_test tests/ode_math_regression.cpp) + add_dependencies(grSim_ode_math_fallback_test grSim_ode_kernel_headers) + set_target_properties(grSim_ode_math_fallback_test PROPERTIES AUTOMOC OFF AUTORCC OFF) + target_compile_definitions(grSim_ode_math_fallback_test PRIVATE GRSIM_ODE_USE_BLAS=0) + target_include_directories(grSim_ode_math_fallback_test PRIVATE "${kernel_test_dir}" + "${PROJECT_SOURCE_DIR}/cmake/ode") + target_link_libraries(grSim_ode_math_fallback_test Threads::Threads) + add_test(NAME ode_math_fallback_regression COMMAND grSim_ode_math_fallback_test) + set_tests_properties(ode_math_fallback_regression PROPERTIES TIMEOUT 30) + if(GRSIM_ODE_MATH_AVAILABLE) + add_executable(grSim_ode_math_test tests/ode_math_regression.cpp) + add_dependencies(grSim_ode_math_test grSim_ode_kernel_headers) + set_target_properties(grSim_ode_math_test PROPERTIES AUTOMOC OFF AUTORCC OFF) + target_compile_definitions(grSim_ode_math_test PRIVATE GRSIM_ODE_USE_BLAS=1) + target_include_directories(grSim_ode_math_test PRIVATE "${kernel_test_dir}" + "${PROJECT_SOURCE_DIR}/cmake/ode" ${GRSIM_ODE_MATH_INCLUDE_DIR}) + target_link_libraries(grSim_ode_math_test ${GRSIM_ODE_MATH_LIBRARIES} Threads::Threads) + add_test(NAME ode_math_regression COMMAND grSim_ode_math_test) + set_tests_properties(ode_math_regression PROPERTIES TIMEOUT 30) + endif() + endif() + add_executable(grSim_ode_test tests/ode_regression.cpp) + target_link_libraries(grSim_ode_test grSim_core) + add_test(NAME ode_regression COMMAND grSim_ode_test) + set_tests_properties(ode_regression PROPERTIES + ENVIRONMENT "QT_QPA_PLATFORM=offscreen" TIMEOUT 180) + add_executable(grSim_collision_filtering_test tests/collision_filtering_regression.cpp) + target_link_libraries(grSim_collision_filtering_test grSim_core) + add_test(NAME collision_filtering_regression COMMAND grSim_collision_filtering_test) + set_tests_properties(collision_filtering_regression PROPERTIES TIMEOUT 30) + add_executable(grSim_physics_defaults_test tests/physics_defaults_regression.cpp) + target_link_libraries(grSim_physics_defaults_test grSim_core) + add_test(NAME physics_defaults_regression COMMAND grSim_physics_defaults_test) + set_tests_properties(physics_defaults_regression PROPERTIES + ENVIRONMENT "QT_QPA_PLATFORM=offscreen" TIMEOUT 30) + add_executable(grSim_physics_experiments tests/physics_experiments.cpp) + target_link_libraries(grSim_physics_experiments grSim_core) + add_executable(grSim_ode_performance_experiments tests/ode_performance_experiments.cpp) + target_link_libraries(grSim_ode_performance_experiments grSim_core) + if(TARGET ode_external) + target_include_directories(grSim_ode_performance_experiments PRIVATE + "${PROJECT_SOURCE_DIR}/cmake/ode" ${GRSIM_ODE_MATH_INCLUDE_DIR}) + target_compile_definitions(grSim_ode_performance_experiments PRIVATE + GRSIM_ODE_USE_BLAS=$) + endif() + find_program(GRSIM_PYTHON3_EXECUTABLE python3) + if(GRSIM_PYTHON3_EXECUTABLE) + add_test(NAME collision_filtering_trajectories + COMMAND ${GRSIM_PYTHON3_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tests/check_collision_filtering.py + $) + set_tests_properties(collision_filtering_trajectories PROPERTIES TIMEOUT 180) + add_test(NAME physics_analytic_regression + COMMAND ${GRSIM_PYTHON3_EXECUTABLE} ${PROJECT_SOURCE_DIR}/tests/check_physics_experiments.py + $) + set_tests_properties(physics_analytic_regression PROPERTIES TIMEOUT 60) + endif() endif() if(APPLE AND GRSIM_MACOSX_BUNDLE) @@ -396,7 +485,9 @@ set(CPACK_RESOURCE_FILE_README "${CMAKE_BINARY_DIR}/README.txt") #set(CPACK_RESOURCE_FILE_WELCOME "${CMAKE_SOURCE_DIR}/WELCOME.txt") set(CPACK_PACKAGE_VERSION ${VERSION}) # Debian based specific -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libode1 (>=0.11), vartypes (>=0.7.0)") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "vartypes (>=0.7.0)") +# Derive shared-library dependencies, including ODE only for a system build. +set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON) if(FLAVOR) set(CPACK_PACKAGE_FILE_NAME "${CMAKE_PROJECT_NAME_LOWER}_${CPACK_PACKAGE_VERSION}-${FLAVOR}_${ARCH}") else() diff --git a/Dockerfile b/Dockerfile index 727de063..25e6c5d8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -12,7 +12,6 @@ RUN apt-get update && apt-get install -y \ libglu1-mesa-dev \ libprotobuf-dev \ protobuf-compiler \ - libode-dev \ libboost-dev WORKDIR /vartypes @@ -34,7 +33,7 @@ COPY src /grsim/src COPY CMakeLists.txt README.md LICENSE.md /grsim/ RUN mkdir build && \ cd build && \ - cmake -DCMAKE_INSTALL_PREFIX=/usr/local .. && \ + cmake -DCMAKE_INSTALL_PREFIX=/usr/local -DCMAKE_BUILD_TYPE=Release -DBUILD_ODE=ON .. && \ make && \ make install @@ -47,7 +46,6 @@ RUN apt-get update && apt-get install -y \ tini \ qt5-default \ libqt5opengl5 \ - libode8 \ libprotobuf17 \ # virtual display and VNC server x11vnc xvfb && \ diff --git a/INSTALL.md b/INSTALL.md index f094c22b..9e75b854 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -14,14 +14,17 @@ GrSim depends on: - [pkg-config](https://www.freedesktop.org/wiki/Software/pkg-config/) - [OpenGL](https://www.opengl.org) - [Qt5 Development Libraries](https://www.qt.io) -- [Open Dynamics Engine (ODE)](http://www.ode.org) +- [Open Dynamics Engine (ODE)](https://www.ode.org/) 0.16.6 or newer, double precision - [VarTypes Library](https://github.com/jpfeltracco/vartypes) forked from [Szi's Vartypes](https://github.com/szi/vartypes) - [Google Protobuf](https://github.com/google/protobuf) - [Boost development libraries](http://www.boost.org/) (needed by VarTypes) -**Note:** It's necessary to compile ODE in double precision. This is default when installing the ODE binaries in Ubuntu. However, if you are compiling ODE from source (e.g on Mac OS), please make sure to enable the double precision during the configuration step: `./configure --enable-double-precision`. - - +grSim uses a compatible system ODE when available. Otherwise, CMake downloads and +builds the pinned 0.16.6 release in double precision. Pass `-DBUILD_ODE=ON` to +select this bundled build explicitly. It also enables ODE's threading interface +and collision thread-local storage for independent concurrent worlds. The first +build needs Git and access to the upstream repository. Installing a distribution's +ODE package is optional when using the bundled build. ## Run from pre-build packages ### Installing from Arch Linux package manager @@ -62,7 +65,7 @@ For Debian, or derivative ``` sudo apt install git build-essential cmake pkg-config qtbase5-dev \ libqt5opengl5-dev libgl1-mesa-dev libglu1-mesa-dev \ - libprotobuf-dev protobuf-compiler libode-dev libboost-dev + libprotobuf-dev protobuf-compiler libode-dev libboost-dev libopenblas-dev ``` #### Mac OS X @@ -126,7 +129,7 @@ $ cd build Run CMake to generate the makefile (note: if you proceed with the installation, grSim will be installed into directory chosen, by default `/usr/local`): ```bash -$ cmake -DCMAKE_INSTALL_PREFIX=/usr/local .. +$ cmake -DCMAKE_INSTALL_PREFIX=/usr/local -DCMAKE_BUILD_TYPE=Release -DBUILD_ODE=ON .. ``` Then compile the program: @@ -163,7 +166,58 @@ grSim will be — by default — installed on the `/usr/local` directory. If you face any problem regarding of updating the grsim version, you can try removing the `grsim.xml`. If grSim crashes almost instantly with some ODE error the issue might by your ODE version. -Try adding -DBUILD_ODE=TRUE to build ODE from source instead of using the system dependency. +Try adding `-DBUILD_ODE=ON` to build the pinned ODE dependency from source. + +## Ball physics defaults + +Ground rolling/spin resistance applies through ball-ground contacts. Under +Physics → Ball, **Ball rolling deceleration (m/s^2)** defaults to 0.35 m/s². +Spin resistance and held-ball traction retain the Ball-ground friction control; +the published rolling model follows the rolling-deceleration setting. +Slip and linear/angular damping default to zero. A one-time migration replaces +the old unused defaults (slip 1 and damping 0.004); other custom values are kept. +Nonzero body damping affects flight as well as ground motion. + +## ODE CPU workers + +Physics and networking run on a simulation worker independently of the display. +For games with at least six robots per team and at least three available logical +CPUs, the app automatically uses up to three ODE helpers, leaving one logical CPU +available for other work. Smaller games and lower-core systems stay serial. + +Helpers process independent contact islands. Collision detection stays on the +simulation worker; contacts that connect robots stay in the same island. +Preparation and solving within each island remain serial, using the direct solver +and the same five substeps, contact geometry and materials. + +GRSIM_ODE_THREADS explicitly overrides automatic selection, including after world +resets. It accepts 0 to 16 helpers; zero runs stepping on the simulation worker alone. + +```bash +GRSIM_ODE_THREADS=0 ./bin/grSim # explicit serial mode on Linux +GRSIM_ODE_THREADS=2 ./bin/grSim.app/Contents/MacOS/grSim # explicit macOS override +``` + +Helper scheduling can change subsequent contact ordering, so crowded trajectories +can diverge numerically. Use GRSIM_ODE_THREADS=0 for reproducible comparisons. +The offline performance harness selects workers explicitly with --helpers. + +## CPU math kernels in bundled ODE + +The bundled ODE build uses double-precision Accelerate on macOS and OpenBLAS +when available on other platforms for triangular solves and dot products with +at least 64 entries. On x86-64 CPUs with AVX2, contiguous triangular solves +read ODE's matrix layout directly with vectorized kernels instead of packing +through BLAS. CPU support is checked at runtime; other CPUs and strided solves +retain the platform BLAS path. Small kernels retain ODE's implementation. These synchronous +CPU calls keep the direct solver, collision shapes, materials and five substeps. +OpenBLAS runs with one active math thread per calling ODE worker. + +On Ubuntu, install libopenblas-dev before configuring. CMake prints the selected +backend; the performance harness reports the BLAS provider as math_backend and +large contiguous triangular dispatch as triangular_backend. Set +GRSIM_ODE_OPTIMIZED_MATH=OFF when configuring to compare the scalar fallback. +A compatible system ODE is used without this bundled patch. ## Notes on the performance @@ -191,7 +245,8 @@ To build locally with Xcode Command Line Tools and Homebrew: bash tests/package_macos.sh build The packaged app uses ad-hoc signing; Developer ID signing and notarization -are not configured. The runtime check exercises normal/Low Spec OpenGL images, +are not configured. Browser downloads may require +[Open Anyway](https://support.apple.com/en-us/102445). The runtime check exercises normal/Low Spec OpenGL images, robot configuration and textures, advancing vision packets, robot commands and feedback, motion and clean worker shutdown. It does not establish visual fidelity or performance on every Apple Silicon model. Intel/universal builds are not diff --git a/README.md b/README.md index 71c0cc6f..cd05c8b9 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,7 @@ grSim compiles on Linux (tested on Ubuntu and Arch Linux variants only) and Mac - [pkg-config](https://freedesktop.org/wiki/Software/pkg-config/) - [OpenGL](https://www.opengl.org) - [Qt5 Development Libraries](https://www.qt.io) -- [Open Dynamics Engine (ODE)](http://www.ode.org) +- [Open Dynamics Engine (ODE)](https://www.ode.org/) 0.16.6+, double precision, with a pinned source fallback - [VarTypes Library](https://github.com/jpfeltracco/vartypes) forked from [Szi's Vartypes](https://github.com/szi/vartypes) - [Google Protobuf](https://github.com/google/protobuf) - [Boost development libraries](http://www.boost.org/) (needed by VarTypes) @@ -78,6 +78,12 @@ outgoing vision packets, plus wall placement after live settings edits. Collision tests check side and rear ball contact, dribbler access, roller retention while stationary and moving, and flat/chip kicks for separate team sizes and rotated robots, including ball acquisition with recessed dribblers. +Robot-pair regressions also cover side/rear pushing, rotated and different-sized +robots, with serial and parallel ODE stepping. Robot contact spheres match the +outer shell radius and attach directly to the chassis, avoiding an extra fixed +body while preserving the previous combined mass and inertia. The kinematic +ball/hull fixtures isolate wheel-ground and rolling constraints; dynamic physics +and robot-pair tests retain the production defaults. The ball-contact hull keeps the body outline and cuts its front back when needed to reach the configured dribbler. Render quality does not change this shape. Tests use temporary configuration values without reading or writing personal @@ -85,6 +91,29 @@ simulator settings. In the configuration tree, **Desired FPS** caps rendering an **ODE time step** sets the fixed physics step; their names remain stable for saved settings compatibility. +Kernel regressions compare native CPU math and scalar fallback against the pinned +ODE implementation, including equation residuals, strides and concurrent callers. +The ODE regressions cover ball resistance, cylinder inertia, settings migration, +collision-filtered contacts/trajectories, helper stepping and concurrent worlds. +The app selects CPU helpers automatically for large games on multicore machines; +see the [worker policy and overrides](INSTALL.md#ode-cpu-workers). +Ball physics defaults and migration are described in the +[installation guide](INSTALL.md#ball-physics-defaults). + +For a repeatable physics-only benchmark after building the tests (6 v 6 by default): + + python3 tests/benchmark_ode.py bin/grSim_ode_test --repeats 3 --steps 300 --output /tmp/grsim-ode-benchmark.json + +For sustained opposing-team contact on a Division B field: + + QT_QPA_PLATFORM=offscreen ./bin/grSim_ode_performance_experiments --scenario pushing-6 --helpers 3 --islands 3 --steps 300 + +The performance harness also provides spaced-6 and crowded-6; each uses twelve +robots total. Add `--audit 1` to report maximum planar robot-shell overlap and +contact/constraint diagnostics. The six-a-side regression rejects overlap above +2 mm. These benchmarks exclude rendering, networking and real-time +throttling. Compare builds using the same geometry, physics settings and workload. + Star History ------ [![Star History Chart](https://api.star-history.com/svg?repos=robocup-ssl/grsim&type=Date)](https://star-history.com/#robocup-ssl/grsim&Date) diff --git a/cmake/modules/BuildODE.cmake b/cmake/modules/BuildODE.cmake index b66957e3..abd43d3f 100644 --- a/cmake/modules/BuildODE.cmake +++ b/cmake/modules/BuildODE.cmake @@ -1,29 +1,87 @@ -# build ODE, because some versions of it cause grSim to segfault somewhere -# could be because in some packages the double precision is turned off include(ExternalProject) +find_package(Threads REQUIRED) +find_package(Git REQUIRED) +option(GRSIM_ODE_OPTIMIZED_MATH "Use native CPU math for large bundled ODE kernels" ON) +set(GRSIM_ODE_MATH_AVAILABLE FALSE) +set(GRSIM_ODE_MATH_NAME ODE) +set(GRSIM_ODE_MATH_LIBRARIES "") +set(GRSIM_ODE_MATH_INCLUDE_DIR "") +if(GRSIM_ODE_OPTIMIZED_MATH) + if(APPLE) + find_library(GRSIM_ACCELERATE_LIBRARY Accelerate) + if(GRSIM_ACCELERATE_LIBRARY) + set(GRSIM_ODE_MATH_AVAILABLE TRUE) + set(GRSIM_ODE_MATH_NAME Accelerate) + set(GRSIM_ODE_MATH_LIBRARIES "${GRSIM_ACCELERATE_LIBRARY}") + endif() + else() + find_path(GRSIM_OPENBLAS_INCLUDE_DIR cblas.h PATH_SUFFIXES openblas openblas-pthread) + find_library(GRSIM_OPENBLAS_LIBRARY NAMES openblas PATH_SUFFIXES openblas-pthread) + if(GRSIM_OPENBLAS_INCLUDE_DIR AND GRSIM_OPENBLAS_LIBRARY) + set(GRSIM_ODE_MATH_AVAILABLE TRUE) + set(GRSIM_ODE_MATH_NAME OpenBLAS) + set(GRSIM_ODE_MATH_LIBRARIES "${GRSIM_OPENBLAS_LIBRARY}") + set(GRSIM_ODE_MATH_INCLUDE_DIR "${GRSIM_OPENBLAS_INCLUDE_DIR}") + endif() + endif() +endif() +mark_as_advanced(GRSIM_OPENBLAS_INCLUDE_DIR GRSIM_OPENBLAS_LIBRARY GRSIM_ACCELERATE_LIBRARY) +set(grsim_ode_patch_dir "${CMAKE_CURRENT_LIST_DIR}/../ode") +set(ode_build_type "${CMAKE_BUILD_TYPE}") +if(NOT ode_build_type) + set(ode_build_type Release) +endif() + +# Pin the 0.16.6 release, including its threading interface and collision TLS. ExternalProject_Add(ode_external - GIT_REPOSITORY https://bitbucket.org/odedevs/ode.git - GIT_TAG 0.16.4 + GIT_REPOSITORY https://github.com/odedevs/ode.git + GIT_TAG a5fc0bc282703ab34d7d3d2500bafae7428eeaa9 + UPDATE_DISCONNECTED TRUE CMAKE_ARGS ${GRSIM_EXTERNAL_PLATFORM_ARGS} - # ODE 0.16.4 requests policies removed by CMake 4. + # Keep compatibility with the dependency on CMake 4. -DCMAKE_POLICY_VERSION_MINIMUM=3.5 -DCMAKE_INSTALL_PREFIX:PATH= + -DCMAKE_INSTALL_LIBDIR:PATH=${CMAKE_INSTALL_LIBDIR} -DCMAKE_TOOLCHAIN_FILE:PATH=${CMAKE_TOOLCHAIN_FILE} -DCMAKE_C_COMPILER:PATH=${CMAKE_C_COMPILER} -DCMAKE_CXX_COMPILER:PATH=${CMAKE_CXX_COMPILER} -DCMAKE_MAKE_PROGRAM:PATH=${CMAKE_MAKE_PROGRAM} - # necessary, because it does not build the static libs if this is ON + -DCMAKE_BUILD_TYPE:STRING=${ode_build_type} + -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON -DBUILD_SHARED_LIBS=OFF - # grSim needs the library; ODE's demos fail to compile with Clang. - -DODE_WITH_DEMOS=OFF - # if this is OFF grSim just dies instantly and INSTALL.md says it should be ON + -DGRSIM_ODE_USE_BLAS:BOOL=${GRSIM_ODE_MATH_AVAILABLE} + -DGRSIM_ODE_BLAS_LIBRARY:FILEPATH=${GRSIM_ODE_MATH_LIBRARIES} + -DGRSIM_ODE_BLAS_INCLUDE_DIR:PATH=${GRSIM_ODE_MATH_INCLUDE_DIR} -DODE_DOUBLE_PRECISION=ON - -DCMAKE_INSTALL_PREFIX= + -DODE_WITH_DEMOS=OFF + -DODE_WITH_TESTS=OFF + -DODE_WITH_OU=ON + -DODE_NO_THREADING_INTF=OFF + -DODE_NO_BUILTIN_THREADING_IMPL=OFF + -DODE_WIN32_LIB_OUTPUT_NAME_BASED_ON_FLOAT_SIZE=OFF + -DODE_WIN32_STATIC_LIB_POSTFIX_DEBUG:STRING= + -DODE_WIN32_STATIC_LIB_POSTFIX_MINSIZEREL:STRING= + -DODE_WIN32_STATIC_LIB_POSTFIX_RELEASE:STRING= + -DODE_WIN32_STATIC_LIB_POSTFIX_RELWITHDEBINFO:STRING= STEP_TARGETS install ) +ExternalProject_Add_Step(ode_external grsim_math_patch + COMMAND "${CMAKE_COMMAND}" "-DSOURCE_DIR=" + "-DPATCH_FILE=${grsim_ode_patch_dir}/cpu-math.patch" + "-DHELPER_FILE=${grsim_ode_patch_dir}/grsim_ode_math.h" + "-DGIT_EXECUTABLE=${GIT_EXECUTABLE}" + -P "${grsim_ode_patch_dir}/ApplyPatch.cmake" + DEPENDEES patch + DEPENDERS configure + DEPENDS "${grsim_ode_patch_dir}/cpu-math.patch" + "${grsim_ode_patch_dir}/grsim_ode_math.h" + "${grsim_ode_patch_dir}/ApplyPatch.cmake") +ExternalProject_Get_Property(ode_external source_dir) +set(GRSIM_BUNDLED_ODE_SOURCE_DIR "${source_dir}") + set(ODE_LIB_SUBPATH "${CMAKE_INSTALL_LIBDIR}/${CMAKE_STATIC_LIBRARY_PREFIX}ode${CMAKE_STATIC_LIBRARY_SUFFIX}") # the byproducts are available after the install step @@ -34,6 +92,17 @@ ExternalProject_Add_Step(ode_external out ) ExternalProject_Get_Property(ode_external install_dir) -set(ODE_LIBRARY "${install_dir}/${ODE_LIB_SUBPATH}") -list(APPEND libs ${ODE_LIBRARY}) -target_include_directories(${app} PRIVATE "${install_dir}/include") +# Imported include directories must exist before the external installation. +file(MAKE_DIRECTORY "${install_dir}/include") +add_library(ode::ode STATIC IMPORTED) +set_target_properties(ode::ode PROPERTIES + IMPORTED_LOCATION "${install_dir}/${ODE_LIB_SUBPATH}" + INTERFACE_INCLUDE_DIRECTORIES "${install_dir}/include" + INTERFACE_COMPILE_DEFINITIONS "dIDEDOUBLE;GRSIM_ODE_MATH_BACKEND=\"${GRSIM_ODE_MATH_NAME}\"" + INTERFACE_LINK_LIBRARIES "Threads::Threads;${GRSIM_ODE_MATH_LIBRARIES}" +) +add_dependencies(ode::ode ode_external) +list(APPEND libs ode::ode) +message(STATUS "Building bundled ODE 0.16.6 (${ode_build_type}, double precision, threading enabled)") + +message(STATUS "Bundled ODE large CPU math kernels: ${GRSIM_ODE_MATH_NAME}") diff --git a/cmake/modules/FindODE.cmake b/cmake/modules/FindODE.cmake index e11f477c..0ba6b75f 100644 --- a/cmake/modules/FindODE.cmake +++ b/cmake/modules/FindODE.cmake @@ -27,16 +27,20 @@ find_path(ODE_INCLUDE_DIR set(ODE_VERSION "${pc_ode_VERSION}") set(ODE_PRECISION "${pc_ode_precision}") +set(ODE_HAS_DOUBLE_PRECISION FALSE) +if(ODE_PRECISION STREQUAL "dDOUBLE") + set(ODE_HAS_DOUBLE_PRECISION TRUE) +endif() include(FindPackageHandleStandardArgs) find_package_handle_standard_args(ODE - REQUIRED_VARS ODE_LIBRARIES ODE_INCLUDE_DIR ODE_PRECISION + REQUIRED_VARS ODE_LIBRARIES ODE_INCLUDE_DIR ODE_HAS_DOUBLE_PRECISION VERSION_VAR ODE_VERSION ) mark_as_advanced(ODE_LIBRARIES ODE_INCLUDE_DIR ODE_PRECISION) -if(ODE_LIBRARIES AND NOT TARGET ode::ode) +if(ODE_FOUND AND NOT TARGET ode::ode) add_library(ode::ode UNKNOWN IMPORTED) set_target_properties(ode::ode PROPERTIES IMPORTED_LOCATION "${ODE_LIBRARIES}" diff --git a/cmake/ode/ApplyPatch.cmake b/cmake/ode/ApplyPatch.cmake new file mode 100644 index 00000000..80f81fbd --- /dev/null +++ b/cmake/ode/ApplyPatch.cmake @@ -0,0 +1,35 @@ +foreach(required SOURCE_DIR PATCH_FILE HELPER_FILE GIT_EXECUTABLE) + if(NOT DEFINED ${required}) + message(FATAL_ERROR "Missing ${required} for bundled ODE math patch") + endif() +endforeach() +set(previous "${SOURCE_DIR}/.grsim-math-applied.patch") +execute_process(COMMAND "${GIT_EXECUTABLE}" apply --reverse --check "${PATCH_FILE}" + WORKING_DIRECTORY "${SOURCE_DIR}" RESULT_VARIABLE already_applied + OUTPUT_QUIET ERROR_QUIET) +if(NOT already_applied EQUAL 0) + if(EXISTS "${previous}") + execute_process(COMMAND "${GIT_EXECUTABLE}" apply --reverse --check "${previous}" + WORKING_DIRECTORY "${SOURCE_DIR}" RESULT_VARIABLE can_remove + OUTPUT_QUIET ERROR_QUIET) + if(can_remove EQUAL 0) + execute_process(COMMAND "${GIT_EXECUTABLE}" apply --reverse "${previous}" + WORKING_DIRECTORY "${SOURCE_DIR}" RESULT_VARIABLE removed) + if(NOT removed EQUAL 0) + message(FATAL_ERROR "Could not update the previous grSim ODE math patch") + endif() + endif() + endif() + execute_process(COMMAND "${GIT_EXECUTABLE}" apply --check "${PATCH_FILE}" + WORKING_DIRECTORY "${SOURCE_DIR}" RESULT_VARIABLE can_apply ERROR_VARIABLE error) + if(NOT can_apply EQUAL 0) + message(FATAL_ERROR "ODE math patch overlaps local dependency changes: ${error}") + endif() + execute_process(COMMAND "${GIT_EXECUTABLE}" apply "${PATCH_FILE}" + WORKING_DIRECTORY "${SOURCE_DIR}" RESULT_VARIABLE applied) + if(NOT applied EQUAL 0) + message(FATAL_ERROR "Could not apply the grSim ODE math patch") + endif() +endif() +configure_file("${PATCH_FILE}" "${previous}" COPYONLY) +configure_file("${HELPER_FILE}" "${SOURCE_DIR}/ode/src/grsim_ode_math.h" COPYONLY) diff --git a/cmake/ode/PrepareKernelTests.cmake b/cmake/ode/PrepareKernelTests.cmake new file mode 100644 index 00000000..165a62bb --- /dev/null +++ b/cmake/ode/PrepareKernelTests.cmake @@ -0,0 +1,22 @@ +file(MAKE_DIRECTORY "${OUTPUT_DIR}") +foreach(name fastlsolve_impl.h fastltsolve_impl.h fastdot_impl.h) + execute_process(COMMAND "${GIT_EXECUTABLE}" show + "a5fc0bc282703ab34d7d3d2500bafae7428eeaa9:ode/src/${name}" + WORKING_DIRECTORY "${SOURCE_DIR}" RESULT_VARIABLE result OUTPUT_VARIABLE reference) + if(NOT result EQUAL 0) + message(FATAL_ERROR "Cannot read pinned ODE kernel reference ${name}") + endif() + file(READ "${SOURCE_DIR}/ode/src/${name}" optimized) + foreach(variant reference optimized) + set(contents "${${variant}}") + string(FIND "${contents}" "template" cooperative_start) + if(NOT cooperative_start EQUAL -1) + string(SUBSTRING "${contents}" 0 ${cooperative_start} contents) + string(APPEND contents "\n#endif\n") + endif() + string(REGEX MATCH "#ifndef [A-Za-z0-9_]+" guard "${contents}") + string(REPLACE "#ifndef " "" guard_name "${guard}") + string(REPLACE "${guard_name}" "${guard_name}_${variant}" contents "${contents}") + file(WRITE "${OUTPUT_DIR}/${variant}_${name}" "${contents}") + endforeach() +endforeach() diff --git a/cmake/ode/cpu-math.patch b/cmake/ode/cpu-math.patch new file mode 100644 index 00000000..7348c3c2 --- /dev/null +++ b/cmake/ode/cpu-math.patch @@ -0,0 +1,71 @@ +--- a/ode/src/fastlsolve_impl.h ++++ b/ode/src/fastlsolve_impl.h +@@ -30,6 +30,8 @@ + #ifndef _ODE_FASTLSOLVE_IMPL_H_ + #define _ODE_FASTLSOLVE_IMPL_H_ + ++#include "grsim_ode_math.h" ++ + + /* solve L*X=B, with B containing 1 right hand sides. + * L is an n*n lower triangular matrix with ones on the diagonal. +@@ -45,6 +47,7 @@ + void solveL1Straight (const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip) + { + dIASSERT(rowCount != 0); ++ if (grsim_ode::solveUnitLower(L, B, rowCount, rowSkip, b_stride, false)) return; + + /* compute all 4 x 1 blocks of X */ + unsigned blockStartRow = 0; +--- a/ode/src/fastltsolve_impl.h ++++ b/ode/src/fastltsolve_impl.h +@@ -31,6 +31,8 @@ + #ifndef _ODE_FASTLTSOLVE_IMPL_H_ + #define _ODE_FASTLTSOLVE_IMPL_H_ + ++#include "grsim_ode_math.h" ++ + + /* solve L^T * x=b, with b containing 1 right hand side. + * L is an n*n lower triangular matrix with ones on the diagonal. +@@ -44,6 +46,7 @@ + void solveL1Transposed(const dReal *L, dReal *B, unsigned rowCount, unsigned rowSkip) + { + dIASSERT(rowCount != 0); ++ if (grsim_ode::solveUnitLower(L, B, rowCount, rowSkip, b_stride, true)) return; + + /* special handling for L and B because we're solving L1 *transpose* */ + const dReal *lastLElement = L + (sizeint)(rowCount - 1) * (rowSkip + 1); +--- a/ode/src/fastdot_impl.h ++++ b/ode/src/fastdot_impl.h +@@ -25,11 +25,14 @@ + #ifndef _ODE_FASTDOT_IMPL_H_ + #define _ODE_FASTDOT_IMPL_H_ + ++#include "grsim_ode_math.h" ++ + + template + dReal calculateLargeVectorDot (const dReal *a, const dReal *b, unsigned n) + { + dReal sum = 0; ++ if (grsim_ode::dot(a, b, n, b_stride, sum)) return sum; + const dReal *a_end = a + (n & (int)(~3)); + for (; a != a_end; b += 4 * b_stride, a += 4) { + dReal p0 = a[0], p1 = a[1], p2 = a[2], p3 = a[3]; +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -1074,3 +1074,13 @@ + endif() + + include(CPack) ++ ++# grSim's optional CPU math backend; the solver and double precision are unchanged. ++option(GRSIM_ODE_USE_BLAS "Use CPU BLAS for large grSim ODE kernels" OFF) ++target_compile_definitions(ODE PRIVATE GRSIM_ODE_USE_BLAS=$) ++if(GRSIM_ODE_USE_BLAS) ++ if(GRSIM_ODE_BLAS_INCLUDE_DIR) ++ target_include_directories(ODE PRIVATE "${GRSIM_ODE_BLAS_INCLUDE_DIR}") ++ endif() ++ target_link_libraries(ODE PUBLIC "${GRSIM_ODE_BLAS_LIBRARY}") ++endif() diff --git a/cmake/ode/grsim_ode_math.h b/cmake/ode/grsim_ode_math.h new file mode 100644 index 00000000..0add6698 --- /dev/null +++ b/cmake/ode/grsim_ode_math.h @@ -0,0 +1,162 @@ +#ifndef GRSIM_ODE_MATH_H +#define GRSIM_ODE_MATH_H + +#include + +#if defined(GRSIM_ODE_USE_BLAS) && GRSIM_ODE_USE_BLAS +# ifdef __APPLE__ +# include +# else +# include +# endif +#endif + + +#if defined(GRSIM_ODE_USE_BLAS) && GRSIM_ODE_USE_BLAS && defined(__x86_64__) && \ + (defined(__GNUC__) || defined(__clang__)) && !defined(__APPLE__) +#include +#define GRSIM_ODE_HAS_AVX2 1 +namespace grsim_ode { +// Four-row solves read ODE's existing row-major factors directly. Runtime +// dispatch keeps baseline x86 CPUs and the platform BLAS fallback usable. +__attribute__((target("avx2"))) inline double horizontalSum(__m256d value) +{ + const __m128d pair = _mm_add_pd(_mm256_castpd256_pd128(value), + _mm256_extractf128_pd(value, 1)); + return _mm_cvtsd_f64(_mm_add_sd(pair, _mm_unpackhi_pd(pair, pair))); +} +__attribute__((target("avx2"))) inline void solveLowerAvx2( + const double* matrix, double* rhs, unsigned count, unsigned rowSkip) +{ + unsigned row = 0; + for (; row + 4 <= count; row += 4) { + const double* first = matrix + std::size_t(row) * rowSkip; + __m256d sum0 = _mm256_setzero_pd(), sum1 = sum0, sum2 = sum0, sum3 = sum0; + for (unsigned col = 0; col < row; col += 4) { + const __m256d values = _mm256_loadu_pd(rhs + col); + sum0 = _mm256_add_pd(sum0, _mm256_mul_pd(_mm256_loadu_pd(first + col), values)); + sum1 = _mm256_add_pd(sum1, _mm256_mul_pd(_mm256_loadu_pd(first + rowSkip + col), values)); + sum2 = _mm256_add_pd(sum2, _mm256_mul_pd(_mm256_loadu_pd(first + 2 * rowSkip + col), values)); + sum3 = _mm256_add_pd(sum3, _mm256_mul_pd(_mm256_loadu_pd(first + 3 * rowSkip + col), values)); + } + rhs[row] -= horizontalSum(sum0); + rhs[row+1] = rhs[row+1] - horizontalSum(sum1) - first[rowSkip+row] * rhs[row]; + rhs[row+2] = rhs[row+2] - horizontalSum(sum2) - + first[2*rowSkip+row] * rhs[row] - first[2*rowSkip+row+1] * rhs[row+1]; + rhs[row+3] = rhs[row+3] - horizontalSum(sum3) - + first[3*rowSkip+row] * rhs[row] - first[3*rowSkip+row+1] * rhs[row+1] - + first[3*rowSkip+row+2] * rhs[row+2]; + } + for (; row < count; ++row) { + double value = rhs[row]; + for (unsigned col = 0; col < row; ++col) + value -= matrix[std::size_t(row)*rowSkip+col] * rhs[col]; + rhs[row] = value; + } +} +__attribute__((target("avx2"))) inline void solveLowerTransposedAvx2( + const double* matrix, double* rhs, unsigned count, unsigned rowSkip) +{ + unsigned end = count; + while (end % 4) { + const unsigned row = --end; + double value = rhs[row]; + for (unsigned col = row + 1; col < count; ++col) + value -= matrix[std::size_t(col)*rowSkip+row] * rhs[col]; + rhs[row] = value; + } + // Each vector spans four adjacent columns of the lower triangle. + while (end) { + const unsigned row = end - 4; + __m256d sum0 = _mm256_setzero_pd(), sum1 = sum0, sum2 = sum0, sum3 = sum0; + unsigned col = end; + for (; col + 4 <= count; col += 4) { + sum0 = _mm256_add_pd(sum0, _mm256_mul_pd( + _mm256_loadu_pd(matrix+std::size_t(col)*rowSkip+row), _mm256_set1_pd(rhs[col]))); + sum1 = _mm256_add_pd(sum1, _mm256_mul_pd( + _mm256_loadu_pd(matrix+std::size_t(col+1)*rowSkip+row), _mm256_set1_pd(rhs[col+1]))); + sum2 = _mm256_add_pd(sum2, _mm256_mul_pd( + _mm256_loadu_pd(matrix+std::size_t(col+2)*rowSkip+row), _mm256_set1_pd(rhs[col+2]))); + sum3 = _mm256_add_pd(sum3, _mm256_mul_pd( + _mm256_loadu_pd(matrix+std::size_t(col+3)*rowSkip+row), _mm256_set1_pd(rhs[col+3]))); + } + sum0 = _mm256_add_pd(_mm256_add_pd(sum0,sum1),_mm256_add_pd(sum2,sum3)); + for (; col < count; ++col) + sum0 = _mm256_add_pd(sum0, _mm256_mul_pd( + _mm256_loadu_pd(matrix+std::size_t(col)*rowSkip+row), _mm256_set1_pd(rhs[col]))); + double sums[4]; _mm256_storeu_pd(sums,sum0); + rhs[row+3] -= sums[3]; + rhs[row+2] = rhs[row+2] - sums[2] - matrix[std::size_t(row+3)*rowSkip+row+2] * rhs[row+3]; + rhs[row+1] = rhs[row+1] - sums[1] - + matrix[std::size_t(row+2)*rowSkip+row+1] * rhs[row+2] - + matrix[std::size_t(row+3)*rowSkip+row+1] * rhs[row+3]; + rhs[row] = rhs[row] - sums[0] - matrix[std::size_t(row+1)*rowSkip+row] * rhs[row+1] - + matrix[std::size_t(row+2)*rowSkip+row] * rhs[row+2] - + matrix[std::size_t(row+3)*rowSkip+row] * rhs[row+3]; + end = row; + } +} +} +#else +#define GRSIM_ODE_HAS_AVX2 0 +#endif + +namespace grsim_ode { +inline const char* triangularBackend() +{ +#if defined(GRSIM_ODE_USE_BLAS) && GRSIM_ODE_USE_BLAS +# if GRSIM_ODE_HAS_AVX2 + if (__builtin_cpu_supports("avx2")) return "AVX2"; +# endif +# ifdef __APPLE__ + return "Accelerate"; +# else + return "OpenBLAS"; +# endif +#else + return "ODE"; +#endif +} +// These synchronous CPU kernels run on the calling simulation/ODE worker. +inline void initializeMath() +{ +#if defined(GRSIM_ODE_USE_BLAS) && GRSIM_ODE_USE_BLAS && !defined(__APPLE__) + // ODE already schedules independent islands. Avoid nested BLAS parallelism. + static const bool initialized = []() { openblas_set_num_threads(1); return true; }(); + (void)initialized; +#endif +} +inline bool solveUnitLower(const double* matrix, double* rhs, unsigned count, + unsigned rowSkip, unsigned rhsStride, bool transposed) +{ +#if defined(GRSIM_ODE_USE_BLAS) && GRSIM_ODE_USE_BLAS + initializeMath(); + if (count >= 64) { +#if GRSIM_ODE_HAS_AVX2 + if (rhsStride == 1 && __builtin_cpu_supports("avx2")) { + if (transposed) solveLowerTransposedAvx2(matrix,rhs,count,rowSkip); + else solveLowerAvx2(matrix,rhs,count,rowSkip); + return true; + } +#endif + cblas_dtrsv(CblasRowMajor, CblasLower, transposed ? CblasTrans : CblasNoTrans, + CblasUnit, count, matrix, rowSkip, rhs, rhsStride); + return true; + } +#endif + return false; +} +inline bool dot(const double* first, const double* second, unsigned count, + unsigned secondStride, double& result) +{ +#if defined(GRSIM_ODE_USE_BLAS) && GRSIM_ODE_USE_BLAS + initializeMath(); + if (count >= 64) { + result = cblas_ddot(count, first, 1, second, secondStride); + return true; + } +#endif + return false; +} +} +#endif diff --git a/include/configwidget.h b/include/configwidget.h index 15eb11a5..58905fa9 100644 --- a/include/configwidget.h +++ b/include/configwidget.h @@ -143,6 +143,7 @@ class ConfigWidget : public VarTreeView DEF_VALUE(double,Double,BallRadius) DEF_VALUE(double,Double,BallMass) DEF_VALUE(double,Double,BallFriction) + DEF_VALUE(double,Double,BallRollingDeceleration) DEF_VALUE(double,Double,BallSlip) DEF_VALUE(double,Double,BallBounce) DEF_VALUE(double,Double,BallBounceVel) diff --git a/include/physics/pcollisionsphere.h b/include/physics/pcollisionsphere.h new file mode 100644 index 00000000..dc9eedfe --- /dev/null +++ b/include/physics/pcollisionsphere.h @@ -0,0 +1,16 @@ +#ifndef PCOLLISIONSPHERE_H +#define PCOLLISIONSPHERE_H + +#include "pobject.h" + +// Geometry only: the attached chassis owns the ODE body. +class PCollisionSphere : public PObject { +public: + PCollisionSphere(dBodyID attachedBody, dReal radius); + void init() override; +private: + dBodyID attachedBody; + dReal radius; +}; + +#endif diff --git a/include/physics/pworld.h b/include/physics/pworld.h index fa16de8e..05453cb8 100644 --- a/include/physics/pworld.h +++ b/include/physics/pworld.h @@ -20,19 +20,39 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #define PWORLD_H #include "pobject.h" -#include #include +#include +#include class PSurface; +class OdeThreading; class PWorld { +public: + struct StepStatistics { + unsigned contacts = 0; + double maxPenetration = 0; + unsigned callbacks = 0, narrowPhaseCalls = 0, createdContacts = 0; + bool measureTimings = false; + double collisionSeconds = 0, narrowPhaseSeconds = 0; + double contactCreationSeconds = 0, solverSeconds = 0; + // Optional offline probes. Timing runs should leave these empty. + std::function contactObserver; + std::function beforeSolve; + }; private: dJointGroupID contactgroup; QVector objects; QVector surfaces; + QVector collisionCategories; + bool collisionFiltering = true; + bool collisionMasksDirty = true; + void updateCollisionMasks(); dReal delta_time; int **sur_matrix; int objects_count; + std::unique_ptr threading; + StepStatistics* activeStatistics = nullptr; public: PWorld(dReal dt,dReal gravity, int robot_count); ~PWorld(); @@ -41,7 +61,17 @@ class PWorld void initAllObjects(); PSurface* createSurface(PObject* o1,PObject* o2); PSurface* findSurface(PObject* o1,PObject* o2); - void step(dReal dt=-1); + // Configure between steps. Zero leaves unclassified objects permissive. + // Masks conservatively admit every registered surface; the matrix still + // decides which individual object pairs may create contacts. + void setCollisionCategory(PObject* object, unsigned long category); + void setCollisionFiltering(bool enabled); + bool collisionFilteringEnabled() const { return collisionFiltering; } + // Statistics accumulate across calls. Zero iterations selects dWorldStep. + bool step(dReal dt=-1, unsigned quickStepIterations=0, StepStatistics* statistics=nullptr); + // Zero runs on the caller. Each island uses one helper by default. + bool setThreadCount(unsigned helpers); + unsigned threadCount() const; std::vector renderPrimitives() const; void handleCollisions(dGeomID o1, dGeomID o2); dWorldID world; @@ -54,7 +84,7 @@ class PSurface { public: PSurface(); - dSurfaceParameters surface; + dSurfaceParameters surface{}; bool isIt(dGeomID i1,dGeomID i2); dGeomID id1,id2; bool usefdir1; //if true use fdir1 instead of ODE value diff --git a/include/robot.h b/include/robot.h index 6671d88d..c55fbaef 100644 --- a/include/robot.h +++ b/include/robot.h @@ -22,6 +22,7 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #include "physics/pworld.h" #include "physics/pcylinder.h" #include "physics/pchassiscollision.h" +#include "physics/pcollisionsphere.h" #include "physics/pbox.h" #include "physics/pball.h" #include "simulationsettings.h" @@ -54,8 +55,7 @@ class Robot { dSpaceID space; PCylinder* chassis; PChassisCollision* ballHull; - PBall* dummy; - dJointID dummy_to_chassis; + PCollisionSphere* dummy; PBox* boxes[3]{}; bool on; //these values are not controled by this class diff --git a/include/scenesnapshot.h b/include/scenesnapshot.h index 7a8f9819..d6a88767 100644 --- a/include/scenesnapshot.h +++ b/include/scenesnapshot.h @@ -41,6 +41,7 @@ struct SceneSnapshot { std::vector primitives; std::vector robots; std::array ballPosition{{0, 0, 0}}; + unsigned odeHelpers = 0; }; using SceneSnapshotPtr = std::shared_ptr; diff --git a/include/simulationsettings.h b/include/simulationsettings.h index d54d1bf5..30e64506 100644 --- a/include/simulationsettings.h +++ b/include/simulationsettings.h @@ -95,6 +95,8 @@ struct SimulationSettings { double BallMass() const { return value_BallMass; } double value_BallFriction{}; double BallFriction() const { return value_BallFriction; } + double value_BallRollingDeceleration = 0.35; + double BallRollingDeceleration() const { return value_BallRollingDeceleration; } double value_BallSlip{}; double BallSlip() const { return value_BallSlip; } double value_BallBounce{}; @@ -109,8 +111,8 @@ struct SimulationSettings { bool BallProjectAirborne() const { return value_BallProjectAirborne; } double value_BallModelTwoPhaseAccSlide{}; double BallModelTwoPhaseAccSlide() const { return value_BallModelTwoPhaseAccSlide; } - double value_BallModelTwoPhaseAccRoll{}; - double BallModelTwoPhaseAccRoll() const { return value_BallModelTwoPhaseAccRoll; } + // The advertised rolling phase must agree with the actual ground model. + double BallModelTwoPhaseAccRoll() const { return -BallRollingDeceleration(); } double value_BallModelTwoPhaseKSwitch{}; double BallModelTwoPhaseKSwitch() const { return value_BallModelTwoPhaseKSwitch; } double value_BallModelChipFixedLossDampingXyFirstHop{}; diff --git a/include/sslworld.h b/include/sslworld.h index f58fb77a..110df5e5 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -48,6 +48,15 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #define WALL_COUNT 10 class RobotsFormation; +// Alternate values support controlled offline experiments without changing +// networking or personal settings. Legacy friction is only a comparison mode. +struct PhysicsStepOptions { + unsigned substeps = 5; + unsigned quickStepIterations = 0; + bool legacyBallFriction = false; + bool finiteWheelRotation = false; +}; + class SendingPacket { public: SendingPacket(SSL_WrapperPacket* _packet,int _t); @@ -62,6 +71,9 @@ class SSLWorld : public QObject int frame_num; dReal last_dt; dReal sim_time = 0; + dSurfaceParameters ballGroundSurface{}; + bool legacyBallFrictionActive = false; + static bool ballGroundContact(dGeomID, dGeomID, PSurface*, int); QList sendQueue; bool lastInfraredState[TEAM_COUNT][MAX_ROBOT_COUNT]{}; KickStatus lastKickState[TEAM_COUNT][MAX_ROBOT_COUNT]{}; @@ -79,6 +91,10 @@ class SSLWorld : public QObject SSLWorld(QObject* parent, SimulationSettings* _cfg, RobotsFormation *form1, RobotsFormation *form2); ~SSLWorld() override; void stepSimulation(dReal dt); + void stepPhysics(dReal dt, const PhysicsStepOptions& options = PhysicsStepOptions(), + PWorld::StepStatistics* statistics = nullptr); + // Apply the same ball material, mass and optional damping on startup/edit. + void updateBallPhysics(); std::shared_ptr snapshot(quint64 generation) const; SSL_WrapperPacket* generatePacket(int cam_id=0); void addFieldLinesArcs(SSL_GeometryFieldSize *field); diff --git a/src/configwidget.cpp b/src/configwidget.cpp index 50604dea..81e00bd6 100644 --- a/src/configwidget.cpp +++ b/src/configwidget.cpp @@ -139,16 +139,20 @@ ConfigWidget::ConfigWidget(bool persist, const QString& settingsPath) phys_vars->addChild(ballp_vars); ADD_VALUE(ballp_vars,Double,BallMass,0.043,"Ball mass"); ADD_VALUE(ballp_vars,Double,BallFriction,0.05,"Ball-ground friction") - ADD_VALUE(ballp_vars,Double,BallSlip,1,"Ball-ground slip") + ADD_VALUE(ballp_vars,Double,BallRollingDeceleration,0.35,"Ball rolling deceleration (m/s^2)") + ADD_VALUE(ballp_vars,Double,BallSlip,0,"Ball-ground slip") ADD_VALUE(ballp_vars,Double,BallBounce,0.5,"Ball-ground bounce factor") ADD_VALUE(ballp_vars,Double,BallBounceVel,0.1,"Ball-ground bounce min velocity") - ADD_VALUE(ballp_vars,Double,BallLinearDamp,0.004,"Ball linear damping") - ADD_VALUE(ballp_vars,Double,BallAngularDamp,0.004,"Ball angular damping") + ADD_VALUE(ballp_vars,Double,BallLinearDamp,0,"Ball linear damping") + ADD_VALUE(ballp_vars,Double,BallAngularDamp,0,"Ball angular damping") ADD_VALUE(ballp_vars,Bool,BallProjectAirborne,true,"Project airborne balls") + auto contactBallDefaults = std::make_shared("Contact ball physics defaults", false); + contactBallDefaults->addFlags(VARTYPE_FLAG_HIDDEN); + ballp_vars->addChild(contactBallDefaults); VarListPtr ball_model_vars(new VarList("Models")); ballp_vars->addChild(ball_model_vars); ADD_VALUE(ball_model_vars,Double,BallModelTwoPhaseAccSlide,-14.0,"Straight-two-phase sliding acceleration") - ADD_VALUE(ball_model_vars,Double,BallModelTwoPhaseAccRoll,-0.7,"Straight-two-phase rolling acceleration") + ADD_VALUE(ball_model_vars,Double,BallModelTwoPhaseAccRoll,-0.35,"Straight-two-phase rolling acceleration") ADD_VALUE(ball_model_vars,Double,BallModelTwoPhaseKSwitch,0.7,"Straight-two-phase switch factor k") ADD_VALUE(ball_model_vars,Double,BallModelChipFixedLossDampingXyFirstHop,0.6,"Chip-fixed-loss damping in xy-direction for first hop") ADD_VALUE(ball_model_vars,Double,BallModelChipFixedLossDampingXyOtherHops,0.96,"Chip-fixed-loss damping in xy-direction for other hops") @@ -183,7 +187,7 @@ ConfigWidget::ConfigWidget(bool persist, const QString& settingsPath) [&]() { fpsLoaded = true; }); const auto stepRead = connect(v_DeltaTime.get(), &VarType::XMLwasRead, this, [&]() { stepLoaded = true; }); - if (persistSettings) + if (persistSettings || !settingsPath.isEmpty()) world=VarXML::read(world,settingsFile.toStdString()); disconnect(fpsRead); disconnect(stepRead); @@ -203,6 +207,18 @@ ConfigWidget::ConfigWidget(bool persist, const QString& settingsPath) const auto synchronization = worldp_vars->findChild("Synchronize ODE with OpenGL"); if (synchronization) worldp_vars->removeChild(synchronization); + // Old startup ignored these stored defaults. Migrate them once, retaining + // other custom values and allowing users to select the old numbers again. + if (!contactBallDefaults->getBool()) { + if (BallSlip() == 1) v_BallSlip->setDouble(0); + if (BallLinearDamp() == 0.004) v_BallLinearDamp->setDouble(0); + if (BallAngularDamp() == 0.004) v_BallAngularDamp->setDouble(0); + contactBallDefaults->setBool(true); + } + v_BallRollingDeceleration->setMin(0); + v_BallModelTwoPhaseAccRoll->setDouble(-BallRollingDeceleration()); + v_BallModelTwoPhaseAccRoll->addFlags(VARTYPE_FLAG_READONLY); + std::string blueteam = v_BlueTeam->getString(); geo_vars->removeChild(v_BlueTeam); @@ -328,6 +344,7 @@ SimulationSettings ConfigWidget::simulationSettings() result.value_BallRadius = BallRadius(); result.value_BallMass = BallMass(); result.value_BallFriction = BallFriction(); + result.value_BallRollingDeceleration = BallRollingDeceleration(); result.value_BallSlip = BallSlip(); result.value_BallBounce = BallBounce(); result.value_BallBounceVel = BallBounceVel(); @@ -335,7 +352,6 @@ SimulationSettings ConfigWidget::simulationSettings() result.value_BallAngularDamp = BallAngularDamp(); result.value_BallProjectAirborne = BallProjectAirborne(); result.value_BallModelTwoPhaseAccSlide = BallModelTwoPhaseAccSlide(); - result.value_BallModelTwoPhaseAccRoll = BallModelTwoPhaseAccRoll(); result.value_BallModelTwoPhaseKSwitch = BallModelTwoPhaseKSwitch(); result.value_BallModelChipFixedLossDampingXyFirstHop = BallModelChipFixedLossDampingXyFirstHop(); result.value_BallModelChipFixedLossDampingXyOtherHops = BallModelChipFixedLossDampingXyOtherHops(); @@ -401,6 +417,7 @@ void ConfigWidget::applyRemoteSettings(const SimulationSettings& settings) void ConfigWidget::queueSettingsEdited() { + v_BallModelTwoPhaseAccRoll->setDouble(-BallRollingDeceleration()); if (settingsNotificationPending) return; settingsNotificationPending = true; QTimer::singleShot(0, this, [this]() { diff --git a/src/physics/pcollisionsphere.cpp b/src/physics/pcollisionsphere.cpp new file mode 100644 index 00000000..525a382b --- /dev/null +++ b/src/physics/pcollisionsphere.cpp @@ -0,0 +1,14 @@ +#include "pcollisionsphere.h" + +PCollisionSphere::PCollisionSphere(dBodyID attachedBody, dReal radius) + : PObject(0, 0, 0, 0, 0, 0, 0), attachedBody(attachedBody), radius(radius) +{ + setVisibility(false); +} + +void PCollisionSphere::init() +{ + geom = dCreateSphere(nullptr, radius); + dGeomSetBody(geom, attachedBody); + dSpaceAdd(space, geom); +} diff --git a/src/physics/pcylinder.cpp b/src/physics/pcylinder.cpp index 4790d788..c00b49c3 100644 --- a/src/physics/pcylinder.cpp +++ b/src/physics/pcylinder.cpp @@ -36,7 +36,8 @@ void PCylinder::setMass(dReal mass) { m_mass = mass; dMass m; - dMassSetCylinderTotal (&m,m_mass,1,m_radius,m_length); + // ODE cylinders extend along local Z, including wheels rotated by the body. + dMassSetCylinderTotal (&m,m_mass,3,m_radius,m_length); dBodySetMass (body,&m); } diff --git a/src/physics/pworld.cpp b/src/physics/pworld.cpp index fd04cda6..a1b371d2 100644 --- a/src/physics/pworld.cpp +++ b/src/physics/pworld.cpp @@ -17,6 +17,110 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) */ #include "pworld.h" +#include +#include +#include +#include +#include +#include +#include + +static_assert(sizeof(dReal) == sizeof(double), "grSim requires double-precision ODE headers"); + +// Each world owns its scheduler, including in serial mode. ODE's implicit +// scheduler is shared globally and cannot serve concurrently stepped worlds. +class OdeThreading { +public: + OdeThreading(dWorldID world, unsigned helpers) : world(world), helpers(helpers) + { + if (!dCheckConfiguration("ODE_EXT_threading")) { + if (helpers) throw std::runtime_error("ODE has no threading interface"); + return; + } + try { + if (helpers && !dCheckConfiguration("ODE_THR_builtin_impl")) + throw std::runtime_error("ODE has no multithreaded implementation"); + implementation = helpers ? dThreadingAllocateMultiThreadedImplementation() + : dThreadingAllocateSelfThreadedImplementation(); + if (!implementation) throw std::runtime_error("Could not allocate ODE scheduler"); + threads.reserve(helpers); + for (unsigned i = 0; i < helpers; ++i) { + threads.emplace_back([this]() { + if (!dAllocateODEDataForThread(dAllocateFlagBasicData)) { + std::lock_guard lock(mutex); + ++failed; + ready.notify_all(); + return; + } + dExternalThreadingServeMultiThreadedImplementation(implementation, + [](void* context) { + auto* self = static_cast(context); + std::lock_guard lock(self->mutex); + ++self->registered; + self->ready.notify_all(); + }, this); + dCleanupODEAllDataForThread(); + }); + } + { + std::unique_lock lock(mutex); + ready.wait(lock, [this]() { return registered + failed == this->helpers; }); + if (failed) throw std::runtime_error("Could not allocate ODE helper data"); + } + dWorldSetStepThreadingImplementation(world, + dThreadingImplementationGetFunctions(implementation), implementation); + attached = true; + dWorldSteppingThreadingParameters parameters = {}; + parameters.param_set = dWSTP_WorldIslandsIterationMaxThreads | + dWSTP_IslandSteppingMaxThreads | dWSTP_LCPSolvingMaxThreads; + parameters.world_islands_iteration_max_threads = helpers ? helpers : 1; + parameters.island_stepping_max_threads = 1; + parameters.lcp_solving_max_threads = 1; + dWorldSetSteppingThreadingParameters(world, ¶meters); + } catch (...) { + stop(); + throw; + } + } + ~OdeThreading() { stop(); } + unsigned threadCount() const { return helpers; } +private: + void stop() + { + if (!implementation) return; + dThreadingImplementationShutdownProcessing(implementation); + for (auto& thread : threads) if (thread.joinable()) thread.join(); + // Detachment frees the world's cached wait object through this scheduler. + if (attached) dWorldSetStepThreadingImplementation(world, nullptr, nullptr); + dThreadingFreeImplementation(implementation); + implementation = nullptr; + } + dWorldID world; + unsigned helpers; + dThreadingImplementationID implementation = nullptr; + bool attached = false; + std::vector threads; + std::mutex mutex; + std::condition_variable ready; + unsigned registered = 0, failed = 0; +}; + +bool PWorld::setThreadCount(unsigned helpers) +{ + if (helpers > 16) return false; + if (threading && threading->threadCount() == helpers) return true; + threading.reset(); + try { + threading.reset(new OdeThreading(world, helpers)); + return true; + } catch (const std::exception&) { + threading.reset(new OdeThreading(world, 0)); + return false; + } +} + +unsigned PWorld::threadCount() const { return threading ? threading->threadCount() : 0; } + PSurface::PSurface() { callback = NULL; @@ -41,16 +145,25 @@ PWorld::PWorld(dReal dt,dReal gravity, int _robot_count) robot_count = _robot_count; world = dWorldCreate(); space = dHashSpaceCreate (0); + dSpaceSetManualCleanup(space, 1); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-gravity); objects_count = 0; sur_matrix = NULL; - //dAllocateODEDataForThread(dAllocateMaskAll); delta_time = dt; + try { + threading.reset(new OdeThreading(world, 0)); + } catch (...) { + dJointGroupDestroy(contactgroup); + dSpaceDestroy(space); + dWorldDestroy(world); + throw; + } } PWorld::~PWorld() { + threading.reset(); dJointGroupDestroy (contactgroup); qDeleteAll(objects); qDeleteAll(surfaces); @@ -67,13 +180,26 @@ void PWorld::setGravity(dReal gravity) void PWorld::handleCollisions(dGeomID o1, dGeomID o2) { + using Clock = std::chrono::steady_clock; + const bool measure = activeStatistics && activeStatistics->measureTimings; + if (activeStatistics) ++activeStatistics->callbacks; PSurface* sur; int j=sur_matrix[*((int*)(dGeomGetData(o1)))][*((int*)(dGeomGetData(o2)))]; if (j!=-1) { const int N = 10; dContact contact[N]; + const auto narrowStart = measure ? Clock::now() : Clock::time_point(); int n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (activeStatistics) { + ++activeStatistics->narrowPhaseCalls; + if (measure) activeStatistics->narrowPhaseSeconds += + std::chrono::duration(Clock::now()-narrowStart).count(); + activeStatistics->contacts += n; + for (int i = 0; i < n; ++i) + activeStatistics->maxPenetration = std::max(activeStatistics->maxPenetration, + double(contact[i].geom.depth)); + } if (n > 0) { sur = surfaces[j]; sur->contactPos [0] = contact[0].geom.pos[0]; @@ -84,6 +210,7 @@ void PWorld::handleCollisions(dGeomID o1, dGeomID o2) sur->contactNormal[2] = contact[0].geom.normal[2]; bool flag=true; if (sur->callback!=NULL) flag = sur->callback(o1,o2,sur,robot_count); + const auto creationStart = measure ? Clock::now() : Clock::time_point(); if (flag) for (int i=0; isurface; @@ -94,12 +221,18 @@ void PWorld::handleCollisions(dGeomID o1, dGeomID o2) contact[i].fdir1[2] = sur->fdir1[2]; contact[i].fdir1[3] = sur->fdir1[3]; } + if (activeStatistics) { + ++activeStatistics->createdContacts; + if (activeStatistics->contactObserver) activeStatistics->contactObserver(contact[i]); + } dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } + if (measure) activeStatistics->contactCreationSeconds += + std::chrono::duration(Clock::now()-creationStart).count(); } } @@ -114,20 +247,20 @@ void PWorld::addObject(PObject* o) o->init(); dGeomSetData(o->geom,(void*)(&(o->id))); objects.append(o); + collisionCategories.append(0); + collisionMasksDirty = true; } void PWorld::initAllObjects() { - objects_count = objects.count(); - int c = objects_count; - bool flag = false; if (sur_matrix!=NULL) { - for (int i=0;iid1)))][*((int*)(dGeomGetData(surfaces[i]->id2)))] = - sur_matrix[(*(int*)(dGeomGetData(surfaces[i]->id2)))][*((int*)(dGeomGetData(surfaces[i]->id1)))] = i; - } + for (int i=0;iid1)))][*((int*)(dGeomGetData(surfaces[i]->id2)))] = + sur_matrix[(*(int*)(dGeomGetData(surfaces[i]->id2)))][*((int*)(dGeomGetData(surfaces[i]->id1)))] = i; + collisionMasksDirty = true; } PSurface* PWorld::createSurface(PObject* o1,PObject* o2) @@ -151,9 +282,50 @@ PSurface* PWorld::createSurface(PObject* o1,PObject* o2) surfaces.append(s); sur_matrix[o1->id][o2->id] = sur_matrix[o2->id][o1->id] = surfaces.count() - 1; + collisionMasksDirty = true; return s; } +void PWorld::setCollisionCategory(PObject* object, unsigned long category) +{ + const int index = objects.indexOf(object); + if (index < 0) throw std::invalid_argument("Collision category requires an owned object"); + collisionCategories[index] = category; + collisionMasksDirty = true; +} + +void PWorld::setCollisionFiltering(bool enabled) +{ + if (collisionFiltering != enabled) { + collisionFiltering = enabled; + collisionMasksDirty = true; + } + updateCollisionMasks(); +} + +void PWorld::updateCollisionMasks() +{ + if (!collisionMasksDirty) return; + QMap allowed; + if (collisionFiltering) for (const auto* surface : surfaces) { + const auto first = collisionCategories[*static_cast(dGeomGetData(surface->id1))]; + const auto second = collisionCategories[*static_cast(dGeomGetData(surface->id2))]; + if (first && second) { + allowed[first] |= second; + allowed[second] |= first; + } + } + for (int i = 0; i < objects.count(); ++i) { + const auto category = collisionCategories[i]; + const bool classified = collisionFiltering && category; + dGeomSetCategoryBits(objects[i]->geom, classified ? category : ~0UL); + dGeomSetCollideBits(objects[i]->geom, classified ? allowed.value(category) : ~0UL); + } + // ODE admits a pair when either direction matches. Unclassified objects + // keep both masks open, so future object types cannot lose contacts. + collisionMasksDirty = false; +} + PSurface* PWorld::findSurface(PObject* o1,PObject* o2) { for (int i=0;imeasureTimings; + const auto collisionStart = measure ? Clock::now() : Clock::time_point(); dSpaceCollide (space,this,&nearCallback); - dWorldStep(world,(dt<0) ? delta_time : dt); - dJointGroupEmpty (contactgroup); - } - catch (...) { - //qDebug() << "Some Error Happened;"; + if (measure) statistics->collisionSeconds += + std::chrono::duration(Clock::now()-collisionStart).count(); + if (statistics && statistics->beforeSolve) statistics->beforeSolve(); + const auto solverStart = measure ? Clock::now() : Clock::time_point(); + const dReal stepSize = (dt<0) ? delta_time : dt; + if (quickStepIterations) { + dWorldSetQuickStepNumIterations(world, quickStepIterations); + success = dWorldQuickStep(world, stepSize) != 0; + } else success = dWorldStep(world, stepSize) != 0; + if (measure) statistics->solverSeconds += + std::chrono::duration(Clock::now()-solverStart).count(); } + catch (...) { success = false; } + dJointGroupEmpty(contactgroup); + activeStatistics = nullptr; + return success; } std::vector PWorld::renderPrimitives() const diff --git a/src/robot.cpp b/src/robot.cpp index 8d7cb225..ad49539b 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -236,14 +236,20 @@ Robot::Robot(PWorld* world,PBall *ball,SimulationSettings* _cfg,dReal x,dReal y, ballHull->space = space; w->addObject(ballHull); - dummy = new PBall(x,y,z,settings.RobotCenterFromKicker,settings.BodyMass*0.01f,0,0,0); - dummy->setVisibility(false); + // The old fixed proxy body was coincident with the chassis. Preserve their + // combined mass and inertia while removing its six redundant joint rows. + dMass chassisMass, proxyMass; + dBodyGetMass(chassis->body, &chassisMass); + dMassSetSphereTotal(&proxyMass, settings.BodyMass*0.01f, settings.RobotCenterFromKicker); + dMassAdd(&chassisMass, &proxyMass); + dBodySetMass(chassis->body, &chassisMass); + + // Robot contact uses the outer shell radius; the separate ball hull still + // provides the dribbler cutout. The sphere follows the chassis automatically. + dummy = new PCollisionSphere(chassis->body, settings.RobotRadius); dummy->space = space; w->addObject(dummy); - dummy_to_chassis = dJointCreateFixed(world->world,0); - dJointAttach (dummy_to_chassis,chassis->body,dummy->body); - kicker = new Kicker(this); wheels[0] = new Wheel(this,0,settings.Wheel1Angle,settings.Wheel1Angle,wheeltexid); @@ -324,8 +330,6 @@ void Robot::resetRobot() resetSpeeds(); dBodySetLinearVel(chassis->body,0,0,0); dBodySetAngularVel(chassis->body,0,0,0); - dBodySetLinearVel(dummy->body,0,0,0); - dBodySetAngularVel(dummy->body,0,0,0); dBodySetLinearVel(kicker->box->body,0,0,0); dBodySetAngularVel(kicker->box->body,0,0,0); for (int i=0;i<4;i++) @@ -373,7 +377,6 @@ void Robot::setXY(dReal x,dReal y) dReal height = ROBOT_START_Z(cfg); chassis->getBodyPosition(xx,yy,zz); chassis->setBodyPosition(x,y,height); - dummy->setBodyPosition(x,y,height); kicker->box->getBodyPosition(kx,ky,kz); kicker->box->setBodyPosition(kx-xx+x,ky-yy+y,kz-zz+height); for (int i=0;i<4;i++) @@ -388,7 +391,6 @@ void Robot::setDir(dReal ang) ang*=M_PI/180.0f; chassis->setBodyRotation(0,0,1,ang); kicker->box->setBodyRotation(0,0,1,ang); - dummy->setBodyRotation(0,0,1,ang); dMatrix3 wLocalRot,wRot,cRot; dVector3 localPos,finalPos,cPos; chassis->getBodyPosition(cPos[0],cPos[1],cPos[2],false); diff --git a/src/simulation.cpp b/src/simulation.cpp index f9f9a2b4..c0dc9843 100644 --- a/src/simulation.cpp +++ b/src/simulation.cpp @@ -71,14 +71,22 @@ void SimulationWorker::start(SimulationSettings value) Q_ASSERT(QThread::currentThread() == QObject::thread()); { QMutexLocker lock(&odeMutex); - if (odeUsers++ == 0) dInitODE2(dInitFlagManualThreadCleanup); + if (odeUsers == 0 && !dInitODE2(dInitFlagManualThreadCleanup)) + qFatal("Could not initialize ODE"); + if (odeUsers > 0 && (!dCheckConfiguration("ODE_EXT_mt_collisions") || + !dCheckConfiguration("ODE_EXT_threading"))) + qFatal("Concurrent worlds require ODE collision TLS and threading; configure BUILD_ODE=ON"); + ++odeUsers; } - dAllocateODEDataForThread(dAllocateMaskAll); + if (!dCheckConfiguration("ODE_double_precision")) qFatal("grSim requires double-precision ODE"); + if (!dAllocateODEDataForThread(dAllocateMaskAll)) qFatal("Could not allocate ODE worker data"); initialized = true; settings = value; if (!std::isfinite(settings.DeltaTime()) || settings.DeltaTime() <= 0) settings.value_DeltaTime = 1.0 / 60.0; settings.value_Robots_Count = qBound(0, settings.Robots_Count(), int(MAX_ROBOT_COUNT)); + if (!std::isfinite(settings.BallRollingDeceleration()) || settings.BallRollingDeceleration() < 0) + settings.value_BallRollingDeceleration = 0.35; settings.value_sendGeometryEvery = qMax(4, settings.sendGeometryEvery()); connectSockets(); rebuildWorld(); @@ -128,6 +136,21 @@ void SimulationWorker::rebuildWorld() delete world; RobotsFormation form(FORMATION_INSIDE_1, &settings); world = new SSLWorld(this, &settings, &form, &form); + const QByteArray requestedThreads = qgetenv("GRSIM_ODE_THREADS"); + if (requestedThreads.isEmpty()) { + const int available = QThread::idealThreadCount(); + // Large games can contain several independent contact islands. Keep + // one logical CPU available for the GUI and bound pool overhead. + const int helpers = settings.Robots_Count() >= 6 && available >= 3 + ? qMin(3, available - 1) : 0; + if (helpers && !world->p->setThreadCount(unsigned(helpers))) + qWarning("Could not start automatic ODE helpers; using serial ODE stepping"); + } else { + bool valid = false; + const int helpers = requestedThreads.toInt(&valid); + if (!valid || helpers < 0 || helpers > 16 || !world->p->setThreadCount(unsigned(helpers))) + qWarning("Could not enable GRSIM_ODE_THREADS; using serial ODE stepping"); + } world->visionServer = visionServer; world->commandSocket = commandSocket; world->simControlSocket = simControlSocket; @@ -152,8 +175,9 @@ void SimulationWorker::configure(SimulationSettingsEdit edit) appliedRevision = edit.revision; SimulationSettings value = edit.after; if (!std::isfinite(value.DeltaTime()) || value.DeltaTime() <= 0 || + !std::isfinite(value.BallRollingDeceleration()) || value.BallRollingDeceleration() < 0 || value.Robots_Count() < 0 || value.Robots_Count() > MAX_ROBOT_COUNT) { - logStatus("Invalid simulation timestep or robot count", QColor("red")); + logStatus("Invalid simulation timestep, rolling deceleration or robot count", QColor("red")); emit settingsChanged(settings, appliedRevision); return; } @@ -170,23 +194,7 @@ void SimulationWorker::configure(SimulationSettingsEdit edit) else { if (previous.Gravity() != settings.Gravity()) dWorldSetGravity(world->p->world, 0, 0, -settings.Gravity()); - if (previous.BallMass() != settings.BallMass()) world->ball->setMass(settings.BallMass()); - if (previous.BallLinearDamp() != settings.BallLinearDamp() || previous.BallAngularDamp() != settings.BallAngularDamp()) { - dBodySetLinearDampingThreshold(world->ball->body, 0.001); - dBodySetLinearDamping(world->ball->body, settings.BallLinearDamp()); - dBodySetAngularDampingThreshold(world->ball->body, 0.001); - dBodySetAngularDamping(world->ball->body, settings.BallAngularDamp()); - } - if (previous.BallFriction() != settings.BallFriction() || - previous.BallSlip() != settings.BallSlip() || previous.BallBounce() != settings.BallBounce() || - previous.BallBounceVel() != settings.BallBounceVel()) { - auto* surface = world->p->findSurface(world->ball, world->ground); - surface->surface.mode = dContactBounce | dContactApprox1 | dContactSlip1 | dContactSlip2; - surface->surface.mu = fric(settings.BallFriction()); - surface->surface.bounce = settings.BallBounce(); - surface->surface.bounce_vel = settings.BallBounceVel(); - surface->surface.slip1 = surface->surface.slip2 = settings.BallSlip(); - } + world->updateBallPhysics(); publish(); } if (timer) timer->setInterval(physicsInterval(settings.DeltaTime())); diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 1cff4622..d469cf53 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -23,6 +23,7 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #include #include +#include #include "logger.h" @@ -83,33 +84,69 @@ bool wheelCallBack(dGeomID o1,dGeomID o2,PSurface* s, int /*robots_count*/) return true; } -bool ballCallBack(dGeomID o1,dGeomID o2,PSurface* s, int /*robots_count*/) +bool SSLWorld::ballGroundContact(dGeomID, dGeomID, PSurface* s, int /*robots_count*/) { auto* world = static_cast(s->callbackData); - if (world->ball->tag!=-1) //spinner adjusting + // A previous held-ball contact must not overwrite the free-ball material. + s->surface = world->ballGroundSurface; + s->usefdir1 = false; + std::fill(s->fdir1, s->fdir1 + 4, 0); + if (world->legacyBallFrictionActive) s->surface.mode &= ~dContactRolling; + const int holder = world->ball->tag; + if (holder >= 0 && holder < world->cfg->Robots_Count() * 2) //spinner adjusting { dReal x,y,z; - world->robots[world->ball->tag]->chassis->getBodyDirection(x,y,z); + world->robots[holder]->chassis->getBodyDirection(x,y,z); s->fdir1[0] = x; s->fdir1[1] = y; s->fdir1[2] = 0; s->fdir1[3] = 0; s->usefdir1 = true; s->surface.mode = dContactMu2 | dContactFDir1 | dContactSoftCFM; - s->surface.mu = world->cfg->BallFriction(); + s->surface.mu = fric(world->cfg->BallFriction()); s->surface.mu2 = 0.5; s->surface.soft_cfm = 0.002; } return true; } +void SSLWorld::updateBallPhysics() +{ + ball->setMass(cfg->BallMass()); + dBodySetLinearDampingThreshold(ball->body, 0.001); + dBodySetAngularDampingThreshold(ball->body, 0.001); + dBodySetLinearDamping(ball->body, cfg->BallLinearDamp()); + dBodySetAngularDamping(ball->body, cfg->BallAngularDamp()); + + ballGroundSurface = {}; + ballGroundSurface.mode = dContactBounce | dContactApprox1 | dContactRolling | dContactMu2; + // For a solid sphere, I/(m*R^2)=2/5 and a=rho*g/(1.4*R). + // Expose a directly so rolling can be tuned without changing sliding, + // spin resistance or tagged held-ball traction. + ballGroundSurface.mu = ballGroundSurface.mu2 = 1; + const dReal rho = cfg->Gravity() > 0 ? + 1.4 * std::max(0.0, cfg->BallRollingDeceleration()) * cfg->BallRadius() / cfg->Gravity() : 0; + ballGroundSurface.rho = ballGroundSurface.rho2 = rho; + ballGroundSurface.rhoN = 2 * fric(cfg->BallFriction()) * cfg->BallRadius(); + ballGroundSurface.bounce = cfg->BallBounce(); + ballGroundSurface.bounce_vel = cfg->BallBounceVel(); + if (cfg->BallSlip() > 0) { + ballGroundSurface.mode |= dContactSlip1 | dContactSlip2; + ballGroundSurface.slip1 = ballGroundSurface.slip2 = cfg->BallSlip(); + } + auto* surface = p->findSurface(ball, ground); + surface->surface = ballGroundSurface; + surface->usefdir1 = false; +} + SSLWorld::SSLWorld(QObject* parent, SimulationSettings* _cfg, RobotsFormation *form1, RobotsFormation *form2) : QObject(parent) { customDT = -1; cfg = _cfg; frame_num = 0; last_dt = cfg->DeltaTime(); - p = new PWorld(0.05,9.81f,cfg->Robots_Count()); + p = new PWorld(0.05,cfg->Gravity(),cfg->Robots_Count()); ball = new PBall (0,0,0.5,cfg->BallRadius(),cfg->BallMass(), 1,0.7,0); + ball->tag = -1; ground = new PGround(cfg->Field_Rad(),cfg->Field_Length(),cfg->Field_Width(),cfg->Field_Penalty_Depth(),cfg->Field_Penalty_Width(),cfg->Field_Penalty_Point(),cfg->Field_Line_Width(),0); @@ -250,9 +287,9 @@ SSLWorld::SSLWorld(QObject* parent, SimulationSettings* _cfg, RobotsFormation *f PSurface wheelswithground; PSurface* ball_ground = p->createSurface(ball,ground); - ball_ground->surface = ballwithwall.surface; - ball_ground->callback = ballCallBack; + ball_ground->callback = ballGroundContact; ball_ground->callbackData = this; + updateBallPhysics(); PSurface ballwithkicker; ballwithkicker.surface.mode = dContactApprox1; @@ -283,11 +320,29 @@ SSLWorld::SSLWorld(QObject* parent, SimulationSettings* _cfg, RobotsFormation *f { if (k != j) { - p->createSurface(robots[k]->dummy,robots[j]->dummy); //seams ode doesn't understand cylinder-cylinder contacts, so I used spheres + p->createSurface(robots[k]->dummy,robots[j]->dummy); // one sphere per outer robot shell p->createSurface(robots[k]->chassis,robots[j]->kicker->box); } } } + enum : unsigned long { + BallCategory = 1UL << 0, GroundCategory = 1UL << 1, + WallCategory = 1UL << 2, ChassisCategory = 1UL << 3, + DummyCategory = 1UL << 4, KickerCategory = 1UL << 5, + WheelCategory = 1UL << 6, HullCategory = 1UL << 7 + }; + p->setCollisionCategory(ball, BallCategory); + p->setCollisionCategory(ground, GroundCategory); + for (auto* wall : walls) p->setCollisionCategory(wall, WallCategory); + for (int k = 0; k < 2 * cfg->Robots_Count(); ++k) { + p->setCollisionCategory(robots[k]->chassis, ChassisCategory); + p->setCollisionCategory(robots[k]->ballHull, HullCategory); + p->setCollisionCategory(robots[k]->dummy, DummyCategory); + p->setCollisionCategory(robots[k]->kicker->box, KickerCategory); + for (auto* wheel : robots[k]->wheels) + p->setCollisionCategory(wheel->cyl, WheelCategory); + } + p->setCollisionFiltering(true); sendGeomCount = 0; // initialize robot state @@ -324,6 +379,7 @@ std::shared_ptr SSLWorld::snapshot(quint64 generation) const result->generation = generation; result->tick = frame_num; result->simulationTime = sim_time; + result->odeHelpers = p->threadCount(); result->settings = *cfg; result->primitives = p->renderPrimitives(); const auto* position = dBodyGetPosition(ball->body); @@ -346,39 +402,57 @@ void SSLWorld::stepSimulation(dReal dt) { if (customDT > 0) dt = customDT; else if (dt <= 0) dt = last_dt; last_dt = dt; + stepPhysics(dt); + sendVisionBuffer(); + frame_num ++; +} - int ballCollisionTry = 5; - for (int kk=0;kk < ballCollisionTry;kk++) { - const dReal* ballvel = dBodyGetLinearVel(ball->body); - dReal ballspeed = ballvel[0]*ballvel[0] + ballvel[1]*ballvel[1] + ballvel[2]*ballvel[2]; - ballspeed = sqrt(ballspeed); - if (ballspeed > 0.01) { - dReal fk = cfg->BallFriction()*cfg->BallMass()*cfg->Gravity(); - dReal ballfx = -fk*ballvel[0] / ballspeed; - dReal ballfy = -fk*ballvel[1] / ballspeed; - dReal ballfz = -fk*ballvel[2] / ballspeed; - dReal balltx = -ballfy*cfg->BallRadius(); - dReal ballty = ballfx*cfg->BallRadius(); - dReal balltz = 0; - dBodyAddTorque(ball->body,balltx,ballty,balltz); - dBodyAddForce(ball->body,ballfx,ballfy,ballfz); - } else { - dBodySetAngularVel(ball->body, 0, 0, 0); - dBodySetLinearVel(ball->body, 0, 0, 0); +void SSLWorld::stepPhysics(dReal dt, const PhysicsStepOptions& options, + PWorld::StepStatistics* statistics) { + if (!std::isfinite(dt) || dt <= 0 || !options.substeps) + throw std::invalid_argument("Invalid physics timestep"); + legacyBallFrictionActive = options.legacyBallFriction; + for (unsigned kk=0; kk < options.substeps; ++kk) { + if (options.finiteWheelRotation) { + for (int k=0; k < cfg->Robots_Count() * 2; ++k) { + for (auto* wheel : robots[k]->wheels) { + dVector3 axis; + dJointGetHingeAxis(wheel->joint, axis); + dBodySetFiniteRotationMode(wheel->cyl->body, 1); + dBodySetFiniteRotationAxis(wheel->cyl->body, axis[0], axis[1], axis[2]); + } + } + } + if (options.legacyBallFriction) { + const dReal* ballvel = dBodyGetLinearVel(ball->body); + dReal ballspeed = ballvel[0]*ballvel[0] + ballvel[1]*ballvel[1] + ballvel[2]*ballvel[2]; + ballspeed = sqrt(ballspeed); + if (ballspeed > 0.01) { + dReal fk = cfg->BallFriction()*cfg->BallMass()*cfg->Gravity(); + dReal ballfx = -fk*ballvel[0] / ballspeed; + dReal ballfy = -fk*ballvel[1] / ballspeed; + dReal ballfz = -fk*ballvel[2] / ballspeed; + dReal balltx = -ballfy*cfg->BallRadius(); + dReal ballty = ballfx*cfg->BallRadius(); + dReal balltz = 0; + dBodyAddTorque(ball->body,balltx,ballty,balltz); + dBodyAddForce(ball->body,ballfx,ballfy,ballfz); + } else { + dBodySetAngularVel(ball->body, 0, 0, 0); + dBodySetLinearVel(ball->body, 0, 0, 0); + } } - p->step(dt/ballCollisionTry); + if (!p->step(dt/options.substeps, options.quickStepIterations, statistics)) + throw std::runtime_error("ODE physics step failed"); } - sim_time += last_dt; + sim_time += dt; ball->tag = -1; for (int k=0;kRobots_Count() * 2;k++) { robots[k]->step(); } - - sendVisionBuffer(); - frame_num ++; } void SSLWorld::addRobotStatus(Robots_Status& robotsPacket, int robotID, bool infrared, KickStatus kickStatus) { diff --git a/tests/benchmark_ode.py b/tests/benchmark_ode.py new file mode 100644 index 00000000..b3b47f8c --- /dev/null +++ b/tests/benchmark_ode.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +"""Compare the headless ODE benchmark modes using interleaved repeated runs.""" +import argparse +from datetime import datetime, timezone +import json +import os +from pathlib import Path +import platform +import random +import statistics +import subprocess + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("executable", type=Path) + parser.add_argument("--baseline", type=Path) + parser.add_argument("--robots", type=int, choices=range(1, 17), default=6, + help="robots per team (default: 6, twelve robots total)") + parser.add_argument("--repeats", type=int, default=3) + parser.add_argument("--steps", type=int, default=300) + parser.add_argument("--output", type=Path, required=True) + args = parser.parse_args() + if args.repeats < 1 or args.steps < 1: + parser.error("repeats and steps must be positive") + modes = [("current-serial", args.executable.resolve(), 0), + ("current-2-helpers", args.executable.resolve(), 2), + ("current-3-helpers", args.executable.resolve(), 3), + ("current-4-helpers", args.executable.resolve(), 4)] + if args.baseline: + modes.insert(0, ("baseline", args.baseline.resolve(), 0)) + cases = [(mode, executable, helpers, robots, crowded) + for mode, executable, helpers in modes + for robots, crowded in [(args.robots, False), (args.robots, True)]] + jobs = [] + randomizer = random.Random(166) + for measurement in range(1, args.repeats + 1): + round_cases = cases.copy() + randomizer.shuffle(round_cases) + jobs.extend((measurement, *case) for case in round_cases) + started_at = datetime.now(timezone.utc).isoformat() + results = [] + for measurement, mode, executable, helpers, robots, crowded in jobs: + command = [str(executable), "--benchmark", "--threads", str(helpers), + "--robots", str(robots), "--steps", str(args.steps)] + if crowded: + command.append("--crowded") + result = json.loads(subprocess.check_output( + command, env=dict(os.environ, QT_QPA_PLATFORM="offscreen"), text=True, timeout=120)) + result["mode"] = mode + result["measurement_round"] = measurement + results.append(result) + print(f"{len(results)}/{len(jobs)} {mode} robots/team={robots} " + f"crowded={crowded} ms/tick={result['ms_per_tick']:.3f}", flush=True) + summaries = [] + for mode, _, helpers in modes: + for robots, crowded in [(args.robots, False), (args.robots, True)]: + samples = [r for r in results if r["mode"] == mode and + r["robots_per_team"] == robots and r["crowded"] == crowded] + serial = next(r for r in results if r["mode"] == "current-serial" and + r["robots_per_team"] == robots and r["crowded"] == crowded) + if any(len(r["state"]) != len(serial["state"]) for r in samples): + raise ValueError("Benchmark state shapes differ between builds") + times = [r["ms_per_tick"] for r in samples] + summaries.append({ + "mode": mode, "ode_version": samples[0]["ode_version"], "helpers": helpers, + "robots_per_team": robots, "crowded": crowded, + "sample_count": len(times), + "mean_ms_per_tick": statistics.mean(times), + "stdev_ms_per_tick": statistics.stdev(times) if len(times) > 1 else 0.0, + "median_ms_per_tick": statistics.median(times), + "min_ms_per_tick": min(times), "max_ms_per_tick": max(times), + "max_state_difference_from_current_serial": max( + max(abs(a-b) for a, b in zip(r["state"], serial["state"])) for r in samples), + "configuration": samples[0]["configuration"], + }) + for result in results: + del result["state"] + cpu_model = platform.processor() + if Path("/proc/cpuinfo").exists(): + cpu_model = next((line.partition(":")[2].strip() for line in + Path("/proc/cpuinfo").read_text().splitlines() + if line.startswith("model name")), cpu_model) + report = {"cpu": cpu_model, "logical_cpus": os.cpu_count(), + "platform": platform.platform(), "steps_per_sample": args.steps, + "warmup_ticks": 60, "repeats": args.repeats, + "started_at_utc": started_at, + "completed_at_utc": datetime.now(timezone.utc).isoformat(), + "execution_order": "Shuffled sequential cases within each measurement round; seed 166.", + "scope": "Collision detection including default ball-ground contacts, dWorldStep and robot motors; excludes rendering, networking, snapshots and real-time throttling.", + "summaries": summaries, "samples": results} + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(json.dumps(report, indent=2) + "\n") + + +if __name__ == "__main__": + main() diff --git a/tests/check_collision_filtering.py b/tests/check_collision_filtering.py new file mode 100644 index 00000000..8e5e3300 --- /dev/null +++ b/tests/check_collision_filtering.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""Check production masks against unfiltered ODE contacts and full body traces.""" +import json +import os +from pathlib import Path +import subprocess +import sys +import tempfile + + +def run(binary, scenario, trace, filtered): + command = [str(binary), "--scenario", scenario, "--steps", "120", + "--audit", "1", "--trace-file", str(trace)] + # Omit --masks for the production-default run. + if not filtered: + command += ["--masks", "0"] + result = subprocess.run(command, check=True, capture_output=True, text=True, + timeout=60, env={**os.environ, "QT_QPA_PLATFORM": "offscreen"}) + data = json.loads(result.stdout) + if not data["success"] or not data["default_filtering_enabled"]: + raise AssertionError(f"{scenario}: production filtering must be enabled by default") + if data["masks"] != filtered: + raise AssertionError(f"{scenario}: requested filtering mode was not applied") + return data + + +def main(): + binary = Path(sys.argv[1]).resolve() + # ConfigWidget resolves robot INI files relative to the executable. + relative = "../config" + if sys.platform.startswith("linux") and str(Path.home()) not in str(binary.parent): + relative = "../share/grSim/config" + if not (binary.parent / relative / "Parsian.ini").is_file(): + raise AssertionError("Missing binary-relative robot configuration") + scenarios = ("spaced-6", "crowded-6", "pushing-6", "spaced-11", "spaced-16", "crowded-11", "crowded-16", + "idle-11", "stopstart-11", "ball-flight", "ball-rolling", + "ball-spin", "dribble-flat", "dribble-chip") + fields = ("contact_hashes", "contact_counts", "narrow_phase_calls", + "detected_contacts", "created_contacts", "max_penetration_m") + with tempfile.TemporaryDirectory(prefix="grsim-collision-") as directory: + traces = [Path(directory) / name for name in ("filtered.f64", "unfiltered.f64")] + for scenario in scenarios: + filtered = run(binary, scenario, traces[0], True) + unfiltered = run(binary, scenario, traces[1], False) + if scenario.endswith("-6") and filtered["diagnostics"]["max_planar_robot_overlap_m"] > 0.002: + raise AssertionError(f"{scenario}: robot shells overlap by more than 2 mm") + for field in fields: + if filtered[field] != unfiltered[field]: + raise AssertionError(f"{scenario}: filtering changed {field}") + actual, expected = (path.read_bytes() for path in traces) + if not actual or actual != expected: + raise AssertionError(f"{scenario}: full body trajectories differ") + if filtered["mask_audit"]["registered_pairs"] != unfiltered["mask_audit"]["registered_pairs"]: + raise AssertionError(f"{scenario}: surface graph changed") + if not scenario.startswith("ball-") and filtered["callbacks"] >= unfiltered["callbacks"]: + raise AssertionError(f"{scenario}: masks did not reduce callbacks") + print(f"PASS {scenario}: identical contacts and trajectories, " + f"callbacks {unfiltered['callbacks']:.0f} -> {filtered['callbacks']:.0f}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/tests/check_physics_experiments.py b/tests/check_physics_experiments.py new file mode 100644 index 00000000..58f9f44a --- /dev/null +++ b/tests/check_physics_experiments.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +"""Check experimental physics against explicit mathematical references.""" +import json +import math +import os +from pathlib import Path +import subprocess +import sys + + +def run(executable, scenario, variant, substeps=5): + result = subprocess.run([str(executable), '--scenario', scenario, '--variant', variant, + '--substeps', str(substeps)], text=True, capture_output=True, + env=dict(os.environ, QT_QPA_PLATFORM='offscreen'), timeout=30) + data = json.loads(result.stdout) + if result.returncode or not data['success']: + raise AssertionError(data.get('error', result.stderr)) + return data + + +def main(): + executable = Path(sys.argv[1]).resolve() + for variant in ['inertia-z', 'system']: + for scenario in ['torque-chassis', 'torque-wheel']: + data = run(executable, scenario, variant) + error = data['metrics']['max_relative_angular_velocity_error'] + assert error < 1e-10, (scenario, error) + print(f'PASS {variant} {scenario}: torque/inertia relative error {error:.3g}') + for variant in ['ball-native', 'system']: + for substeps in [5, 15, 30]: + data = run(executable, 'ball-flight', variant, substeps) + metrics = data['metrics'] + assert metrics['flight_horizontal_error_m'] < 1e-10, metrics + # Semi-implicit Euler's constant-gravity position error. + expected_error = 0.5*data['parameters']['gravity_m_s2']*data['duration_s']/(60*substeps) + assert math.isclose(metrics['flight_vertical_error_m'], expected_error, abs_tol=1e-9), metrics + print(f'PASS {variant} flight, {substeps} substeps: analytic integration error {expected_error:.6g} m') + for variant, scale in [('ball-native-half', 0.5), ('ball-native', 1), ('ball-native-double', 2), ('system', 1)]: + data = run(executable, 'ball-rolling', variant) + metrics = data['metrics'] + expected = (-data['parameters']['ball_rolling_deceleration_m_s2'] if variant == 'system' + else scale*metrics['expected_matched_rolling_acceleration_m_s2']) + assert math.isclose(metrics['mean_rolling_acceleration_m_s2'], expected, rel_tol=1e-6), metrics + print(f'PASS {variant}: steady rolling acceleration {expected:.6g} m/s^2') + for variant in ['ball-native', 'system']: + data = run(executable, 'ball-spin', variant) + metrics = data['metrics'] + assert 0 <= metrics['ball_stop_time_s']-metrics['expected_native_spin_stop_s'] <= 1/60, metrics + print(f'PASS {variant} spin: stop time agrees within one recorded tick') + data = run(executable, 'ball-drop', variant, 30) + time = data['trajectory'][18][0] + gravity = data['parameters']['gravity_m_s2'] + expected_z = 1-0.5*gravity*time*time-0.5*gravity*time/(60*30) + assert math.isclose(data['trajectory'][18][3], expected_z, abs_tol=1e-9), data['trajectory'][18] + print(f'PASS {variant} fine-step drop: release from rest follows gravity before ground impact') + + +if __name__ == '__main__': + main() diff --git a/tests/collision_filtering_regression.cpp b/tests/collision_filtering_regression.cpp new file mode 100644 index 00000000..ced14b78 --- /dev/null +++ b/tests/collision_filtering_regression.cpp @@ -0,0 +1,103 @@ +#include "physics/pball.h" +#include "physics/pworld.h" +#include +#include + +namespace { +void require(bool condition, const char* description) +{ + if (!condition) throw std::runtime_error(description); +} + +unsigned callbacks(PWorld& world) +{ + unsigned count = 0; + dSpaceCollide(world.space, &count, [](void* data, dGeomID, dGeomID) { + ++*static_cast(data); + }); + return count; +} + +void reset(PBall* ball, double x) +{ + dBodySetPosition(ball->body, x, 0, 0); + dBodySetLinearVel(ball->body, 0, 0, 0); + dBodySetAngularVel(ball->body, 0, 0, 0); +} + +void checkFiltering() +{ + PWorld world(1.0 / 300, 0, 0); + auto* first = new PBall(0, 0, 0, 0.02, 1, 1, 1, 1); + auto* second = new PBall(0.02, 0, 0, 0.02, 1, 1, 1, 1); + world.addObject(first); + world.addObject(second); + world.initAllObjects(); + world.setCollisionCategory(first, 1); + world.setCollisionCategory(second, 2); + world.setCollisionFiltering(true); + require(callbacks(world) == 0, "unsupported pair must be rejected before the callback"); + + auto* original = world.createSurface(first, second); + PWorld::StepStatistics lateSurface; + require(world.step(-1, 0, &lateSurface), "step failed after adding a surface"); + require(lateSurface.narrowPhaseCalls == 1 && lateSurface.createdContacts > 0, + "new surfaces must refresh masks before the next collision pass"); + + auto* unknown = new PBall(0.01, 0, 0, 0.02, 1, 1, 1, 1); + world.addObject(unknown); + world.initAllObjects(); + require(world.findSurface(first, second) == original, "matrix rebuild lost an existing surface"); + world.createSurface(first, unknown); + world.setCollisionFiltering(true); + reset(first, 0); + reset(second, 0.02); + require(dGeomGetCategoryBits(unknown->geom) == ~0UL && + dGeomGetCollideBits(unknown->geom) == ~0UL, + "unclassified objects must remain permissive"); + require(callbacks(world) == 3, "unknown objects must pass either direction of ODE filtering"); + PWorld::StepStatistics unclassified; + require(world.step(-1, 0, &unclassified), "step failed with unclassified object"); + require(unclassified.callbacks == 3 && unclassified.narrowPhaseCalls == 2, + "surface matrix must still reject unsupported object pairs"); + + world.setCollisionCategory(unknown, 4); + world.setCollisionFiltering(true); + reset(first, 0); + reset(second, 0.02); + reset(unknown, 0.01); + require(callbacks(world) == 2, "classification must reject the remaining unsupported pair"); + world.setCollisionFiltering(false); + require(callbacks(world) == 3, "disabling filtering must restore all callbacks"); + world.setCollisionFiltering(true); + require(callbacks(world) == 2, "reenabling filtering must restore derived masks"); + + world.createSurface(second, unknown); + PWorld::StepStatistics allPairs; + require(world.step(-1, 0, &allPairs), "step failed after expanding the graph"); + require(allPairs.narrowPhaseCalls == 3 && allPairs.createdContacts == 3, + "late surface graph changes must preserve every allowed contact"); + + bool rejected = false; + try { world.setCollisionCategory(nullptr, 1); } + catch (const std::invalid_argument&) { rejected = true; } + require(rejected, "categories must reject objects outside this world"); +} +} // namespace + +int main() +{ + if (!dInitODE2(dInitFlagManualThreadCleanup)) return 1; + if (!dAllocateODEDataForThread(dAllocateMaskAll)) { dCloseODE(); return 1; } + int status = 0; + try { + checkFiltering(); + std::puts("PASS collision filtering, late surfaces, matrix growth and unknown objects"); + } catch (const std::exception& error) { + std::fprintf(stderr, "FAIL %s\n", error.what()); + status = 1; + } + dCleanupODEAllDataForThread(); + dCloseODE(); + return status; +} diff --git a/tests/collision_regression.cpp b/tests/collision_regression.cpp index 5d93605d..b29de9fd 100644 --- a/tests/collision_regression.cpp +++ b/tests/collision_regression.cpp @@ -2,9 +2,18 @@ #include #include #include +#include +#include #include "configwidget.h" #include "sslworld.h" +static std::atomic lcpErrors{0}; +static void recordOdeMessage(int code, const char* format, va_list arguments) +{ + if (code == d_ERR_LCP) ++lcpErrors; + std::vfprintf(stderr, format, arguments); std::fputc('\n', stderr); +} + static bool check(bool passed, const char* description) { std::printf("%s %s\n", passed ? "PASS" : "FAIL", description); @@ -23,6 +32,10 @@ static void placeBall(SSLWorld& world, double x, double y, double z) static bool checkRobot(SimulationSettings settings, int team, double heading) { + // Isolate hull/ball geometry from rolling constraints against a kinematic + // chassis. Dynamic dribble/rolling and robot-pair tests use full defaults. + settings.value_BallRollingDeceleration = 0; + settings.value_BallFriction = 0; const auto& teamSettings = team ? settings.yellowSettings : settings.blueSettings; std::printf("CHECK team=%d heading=%.0f radius=%.1fmm dribbler=%.1fmm\n", team, heading, teamSettings.RobotRadius*1000, teamSettings.RobotCenterFromKicker*1000); @@ -35,6 +48,15 @@ static bool checkRobot(SimulationSettings settings, int team, double heading) for (int i = 0; i < 200; ++i) world.p->step(1.0/300.0); // Hold the chassis still to measure ball penetration rather than robot pushback. dBodySetKinematic(robot->chassis->body); + // A held chassis does not need driven wheels constrained against the floor. + for (auto* wheel : robot->wheels) { + dJointSetAMotorParam(wheel->motor, dParamFMax, 0); + dGeomDisable(wheel->cyl->geom); + dBodySetGravityMode(wheel->cyl->body, 0); + } + + dBodySetLinearVel(robot->chassis->body,0,0,0); + dBodySetAngularVel(robot->chassis->body,0,0,0); world.p->setGravity(0); const auto* position = dBodyGetPosition(robot->chassis->body); const double centerX = position[0], centerY = position[1]; @@ -55,23 +77,34 @@ static bool checkRobot(SimulationSettings settings, int team, double heading) "existing chassis-to-wall contact remains available") && passed; dGeomSetPosition(wall->geom, oldWall[0], oldWall[1], oldWall[2]); - for (double side : {90.0, 180.0, 270.0}) { - const double angle = (heading + side) * M_PI / 180; - const double x = std::cos(angle), y = std::sin(angle); - const double start = radius + ballRadius + 0.03; - placeBall(world, centerX + x*start, centerY + y*start, ballRadius); - dBodySetLinearVel(world.ball->body, -0.6*x, -0.6*y, 0); - double closest = start; - for (int i = 0; i < 90; ++i) { - world.p->step(1.0/300.0); - const auto* ball = dBodyGetPosition(world.ball->body); - closest = std::min(closest, (ball[0]-centerX)*x + (ball[1]-centerY)*y); + auto sideImpacts = [&](double height) { + bool blockedAll = true; + for (double side : {90.0, 180.0, 270.0}) { + const double angle = (heading + side) * M_PI / 180; + const double x = std::cos(angle), y = std::sin(angle); + const double start = radius + ballRadius + 0.03; + placeBall(world, centerX + x*start, centerY + y*start, height); + dBodySetLinearVel(world.ball->body, -0.6*x, -0.6*y, 0); + double closest = start; + for (int i = 0; i < 90; ++i) { + world.p->step(1.0/300.0); + const auto* ball = dBodyGetPosition(world.ball->body); + closest = std::min(closest, (ball[0]-centerX)*x + (ball[1]-centerY)*y); + } + const bool blocked = closest >= radius + ballRadius - 0.002; + std::printf("%s team=%d heading=%.0f side=%.0f height=%.2fmm closest=%.2fmm expected=%.2fmm\n", + blocked ? "PASS" : "FAIL", team, heading, side, height*1000, closest*1000, (radius+ballRadius)*1000); + blockedAll = blocked && blockedAll; } - const bool blocked = closest >= radius + ballRadius - 0.002; - std::printf("%s team=%d heading=%.0f side=%.0f closest=%.2fmm expected=%.2fmm\n", - blocked ? "PASS" : "FAIL", team, heading, side, closest*1000, (radius+ballRadius)*1000); - passed = blocked && passed; - } + + return blockedAll; + }; + // Separate the geometry-only fixture from an unloaded rolling constraint. + passed = sideImpacts(position[2]) && passed; + // Retain ground-level impacts under a real normal load as well. + world.p->setGravity(settings.Gravity()); + passed = sideImpacts(ballRadius) && passed; + world.p->setGravity(0); const double yaw = heading * M_PI / 180, x = std::cos(yaw), y = std::sin(yaw); const auto* kicker = dBodyGetPosition(robot->kicker->box->body); @@ -143,6 +176,76 @@ static bool checkRobot(SimulationSettings settings, int team, double heading) return passed; } +// Use the production pair registration, motors and five-substep integrator. +static bool checkRobotPair(SimulationSettings settings, double heading, double side, unsigned helpers) +{ + settings.value_Robots_Count = 1; + settings.value_DeltaTime = 1.0/60; + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + if (!check(world.p->setThreadCount(helpers), "robot-pair ODE helpers are available")) return false; + auto* first = world.robots[0]; + auto* second = world.robots[1]; + bool preservedMass = true; + for (auto* robot : {first, second}) { + dMass mass; + dBodyGetMass(robot->chassis->body, &mass); + const auto& shape = robot->settings; + const double cylinder = shape.BodyMass*0.99f, proxy = shape.BodyMass*0.01f; + const double sphereInertia = 0.4*proxy*shape.RobotCenterFromKicker*shape.RobotCenterFromKicker; + const double transverse = cylinder*(3*shape.RobotRadius*shape.RobotRadius+ + shape.RobotHeight*shape.RobotHeight)/12+sphereInertia; + const double axial = 0.5*cylinder*shape.RobotRadius*shape.RobotRadius+sphereInertia; + preservedMass = preservedMass && std::abs(mass.mass-cylinder-proxy) < 1e-12 && + std::abs(mass.I[0]-transverse) < 1e-12 && std::abs(mass.I[5]-transverse) < 1e-12 && + std::abs(mass.I[10]-axial) < 1e-12; + } + if (!check(preservedMass, "compound robot preserves total chassis/proxy mass and inertia")) return false; + first->step(); second->step(); + const double angle = (heading+side)*M_PI/180; + const double x = std::cos(angle), y = std::sin(angle); + const double diameter = first->settings.RobotRadius+second->settings.RobotRadius; + const double start = diameter+0.02; + first->setXY(-x*start/2, -y*start/2); + second->setXY(x*start/2, y*start/2); + first->setDir(heading); second->setDir(heading); + placeBall(world, 2, 2, settings.BallRadius()); + for (int i=0; i<60; ++i) world.stepPhysics(1.0/60); + + unsigned pairContacts = 0; + PWorld::StepStatistics statistics; + statistics.contactObserver = [&](const dContact& contact) { + if ((contact.geom.g1 == first->dummy->geom && contact.geom.g2 == second->dummy->geom) || + (contact.geom.g2 == first->dummy->geom && contact.geom.g1 == second->dummy->geom)) + ++pairContacts; + }; + const double local = side*M_PI/180; + double closest = start; + for (int i=0; i<120; ++i) { + first->setSpeed(0.25*std::cos(local), 0.25*std::sin(local), 0); + second->setSpeed(-0.25*std::cos(local), -0.25*std::sin(local), 0); + world.stepPhysics(1.0/60, PhysicsStepOptions(), &statistics); + const auto* a = dBodyGetPosition(first->chassis->body); + const auto* b = dBodyGetPosition(second->chassis->body); + const double distance = std::hypot(a[0]-b[0], a[1]-b[1]); + if (!std::isfinite(distance)) return check(false, "robot-pair positions stay finite"); + closest = std::min(closest, distance); + } + const bool blocked = pairContacts > 0 && closest >= diameter-0.002; + first->resetRobot(); + first->setXY(1, -1); + first->setDir(heading+45); + const auto* center = dGeomGetPosition(first->dummy->geom); + const bool followsPose = std::hypot(center[0]-1, center[1]+1) < 1e-12; + const bool passed = blocked && followsPose; + std::printf("%s robot-pair radii=%.1f/%.1fmm heading=%.0f side=%.0f helpers=%u " + "closest=%.2fmm expected=%.2fmm contacts=%u\n", + passed ? "PASS" : "FAIL", first->settings.RobotRadius*1000, + second->settings.RobotRadius*1000, heading, side, helpers, + closest*1000, diameter*1000, pairContacts); + return passed; +} + int main(int argc, char** argv) { QApplication app(argc, argv); @@ -153,7 +256,10 @@ int main(int argc, char** argv) settings.yellowSettings.RobotRadius = 0.0793; settings.yellowSettings.RobotCenterFromKicker = 0.068; settings.yellowSettings.RobotHeight = 0.138; - dInitODE2(0); + if (!dInitODE2(dInitFlagManualThreadCleanup) || + !dAllocateODEDataForThread(dAllocateMaskAll)) return 2; + const auto previousMessageHandler = dGetMessageHandler(); + dSetMessageHandler(recordOdeMessage); bool passed = true; for (int team : {0, 1}) for (double heading : {0.0, 90.0}) @@ -165,6 +271,21 @@ int main(int argc, char** argv) for (double heading : {0.0, 90.0}) passed = checkRobot(settings, 0, heading) && passed; } + // Reverse the team sizes to catch either team's collider using shared settings. + for (int sizes=0; sizes<3; ++sizes) { + settings.blueSettings.RobotRadius = sizes == 2 ? 0.0793 : 0.09; + settings.yellowSettings.RobotRadius = sizes == 1 ? 0.0793 : 0.09; + settings.blueSettings.RobotCenterFromKicker = sizes == 2 ? 0.068 : 0.073; + settings.yellowSettings.RobotCenterFromKicker = sizes == 1 ? 0.068 : 0.073; + for (double heading : {0.0, 90.0}) for (double side : {90.0, 180.0}) { + passed = checkRobotPair(settings, heading, side, 0) && passed; + if (dCheckConfiguration("ODE_EXT_threading") && dCheckConfiguration("ODE_THR_builtin_impl")) + passed = checkRobotPair(settings, heading, side, 3) && passed; + } + } + passed = check(lcpErrors == 0, "contact fixtures complete without LCP solver errors") && passed; + dSetMessageHandler(previousMessageHandler); + dCleanupODEAllDataForThread(); dCloseODE(); return passed ? 0 : 1; } diff --git a/tests/ode_math_regression.cpp b/tests/ode_math_regression.cpp new file mode 100644 index 00000000..19c26b17 --- /dev/null +++ b/tests/ode_math_regression.cpp @@ -0,0 +1,162 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "grsim_ode_math.h" + +using dReal = double; +using sizeint = std::size_t; +static void require(bool value, const char* message) +{ + if (!value) throw std::runtime_error(message); +} +#define dIASSERT(value) require((value), #value) +namespace reference { +#include "reference_fastlsolve_impl.h" +#include "reference_fastltsolve_impl.h" +#include "reference_fastdot_impl.h" +} +namespace optimized { +#include "optimized_fastlsolve_impl.h" +#include "optimized_fastltsolve_impl.h" +#include "optimized_fastdot_impl.h" +} +#undef dIASSERT + +static double randomValue(std::uint64_t& state) +{ + state ^= state << 13; state ^= state >> 7; state ^= state << 17; + return double(state % 20001)/10000-1; +} + +template +static double checkSolve(unsigned count, bool transposed, bool sparse, unsigned seed) +{ + const unsigned rowSkip = count+1+seed%4; + const double guard = 1234567.25; + std::uint64_t state = 166+seed; + // Diagonal/upper/padding are deliberately NaN: a unit triangular solve + // must not use them. Exercise non-aligned row pitches and strided RHS data. + std::vector matrix(sizeint(count)*rowSkip, + std::numeric_limits::quiet_NaN()); + for (unsigned row=0; row original(sizeint(count)*stride+4,guard); + for (unsigned i=0; i(matrix.data(),e,count,rowSkip); + optimized::solveL1Transposed(matrix.data(),a,count,rowSkip); + } else { + reference::solveL1Straight(matrix.data(),e,count,rowSkip); + optimized::solveL1Straight(matrix.data(),a,count,rowSkip); + } + double norm=1, difference=0; + for (unsigned i=0; i::epsilon()*count; + require(difference/norm <= tolerance,"triangular solve differs from pinned ODE reference"); + for (unsigned i=0; i=count) + require(actual[i]==guard,"triangular solve overwrote RHS padding"); + // Independently check the equations in long double. + for (unsigned row=0; rowrow)) { + const double coefficient=transposed ? matrix[sizeint(col)*rowSkip+row] : + matrix[sizeint(row)*rowSkip+col]; + const long double term=static_cast(coefficient)*a[sizeint(col)*stride]; + sum+=term; magnitude+=std::abs(term); + } + } + const long double rhs=original[2+sizeint(row)*stride]; + require(std::abs(sum-rhs) <= tolerance*std::max(1.L,magnitude+std::abs(rhs)), + "triangular solution violates original equations"); + } + return difference/norm; +} + +template +static void checkDot(unsigned count) +{ + std::uint64_t state=166+count; + std::vector a(count), b(sizeint(count)*stride+2,777.25); + long double sum=0, magnitude=0; + for (unsigned i=0; i(a[i])*b[sizeint(i)*stride]; + sum+=product; magnitude+=std::abs(product); + } + const double actual=optimized::calculateLargeVectorDot(a.data(),b.data(),count); + const double expected=reference::calculateLargeVectorDot(a.data(),b.data(),count); + const double tolerance=64*std::numeric_limits::epsilon()*count; + require(std::abs(actual-expected) <= tolerance*std::max(1.L,magnitude), + "dot differs from pinned ODE reference"); + require(std::abs(actual-sum) <= tolerance*std::max(1.L,magnitude), + "dot violates long-double reference"); +} + +int main() +{ + try { + double small[4]={1,2,3,4}, smallResult=777; + require(!grsim_ode::dot(small,small,1,1,smallResult) && smallResult==777, + "small dot must retain ODE arithmetic"); +#if GRSIM_ODE_USE_BLAS && !defined(__APPLE__) + require(openblas_get_num_threads()==1,"small kernels must configure one BLAS thread"); +#endif + // Backend calls from independent ODE worlds/islands are concurrent. + std::string errors[8]; + std::vector threads; + for (unsigned i=0; i<8; ++i) threads.emplace_back([&,i]() { + try {checkSolve<1>(65,i%2,true,i);checkDot<2>(129);} + catch (const std::exception& e) {errors[i]=e.what();} + }); + for (auto& thread:threads) thread.join(); + for (const auto& error:errors) require(error.empty(),error.c_str()); + unsigned solves=0; + double largest=0; + for (unsigned n : {1U,2U,3U,4U,5U,7U,12U,15U,16U,31U,32U,33U,63U,64U,65U, + 66U,67U,95U,96U,97U,127U,128U,129U,191U,255U,256U,257U, + 511U,512U,513U,1023U,1024U,1025U}) { + for (bool transposed : {false,true}) for (bool sparse : {false,true}) { + largest=std::max(largest,checkSolve<1>(n,transposed,sparse,n)); + largest=std::max(largest,checkSolve<2>(n,transposed,sparse,n+1)); + solves+=2; + } + checkDot<1>(n);checkDot<2>(n); + } + double rhs[4]={1,2,3,4}, result=777; + require(!grsim_ode::solveUnitLower(rhs,rhs,1,1,1,false) && rhs[0]==1, + "small solve must retain ODE fallback"); + require(!grsim_ode::dot(rhs,rhs,1,1,result) && result==777, + "small dot must retain ODE fallback"); +#if GRSIM_ODE_USE_BLAS && !defined(__APPLE__) + require(openblas_get_num_threads()==1,"BLAS must not nest an ODE worker pool"); +#endif + std::printf("PASS ODE CPU math: %u triangular cases, strided dots, 8 concurrent callers; " + "max normalized reference difference %.3g; triangular backend %s\n", + solves,largest,grsim_ode::triangularBackend()); + return 0; + } catch (const std::exception& error) { + std::fprintf(stderr,"FAIL ODE CPU math: %s\n",error.what()); + return 1; + } +} diff --git a/tests/ode_performance_experiments.cpp b/tests/ode_performance_experiments.cpp new file mode 100644 index 00000000..f3436d10 --- /dev/null +++ b/tests/ode_performance_experiments.cpp @@ -0,0 +1,524 @@ +// Offline experiments using the production masks and optional threading controls. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "configwidget.h" +#include "sslworld.h" +#ifdef GRSIM_ODE_USE_BLAS +#include "grsim_ode_math.h" +#endif + +#ifndef GRSIM_ODE_MATH_BACKEND +#define GRSIM_ODE_MATH_BACKEND "ODE" +#endif + +namespace { +using Clock = std::chrono::steady_clock; +constexpr double tick = 1.0/60; +std::atomic lcpErrors{0}; +void recordOdeMessage(int code, const char* format, va_list arguments) +{ + if (code == d_ERR_LCP) ++lcpErrors; + std::vfprintf(stderr, format, arguments); + std::fputc('\n', stderr); +} +struct Options { + QString scenario = "spaced-6", traceFile, perfControl, perfAck; + unsigned helpers = 0, islands = 1, preparation = 1; + int steps = 300, warmup = 60; + bool masks = true, profile = false, audit = false, replay = false; +}; +struct Object { PObject* object; unsigned category; }; + +std::vector objectsOf(SSLWorld& world) +{ + std::vector objects{{world.ball,0}, {world.ground,1}}; + for (auto* wall : world.walls) objects.push_back({wall,2}); + for (int i=0; i<2*world.cfg->Robots_Count(); ++i) { + auto* robot = world.robots[i]; + objects.push_back({robot->chassis,3}); + objects.push_back({robot->ballHull,7}); + objects.push_back({robot->dummy,4}); + objects.push_back({robot->kicker->box,5}); + for (auto* wheel : robot->wheels) objects.push_back({wheel->cyl,6}); + } + std::sort(objects.begin(), objects.end(), [](const Object& a, const Object& b) { + return a.object->id < b.object->id; + }); + if (objects.size() != size_t(dSpaceGetNumGeoms(world.p->space))) + throw std::runtime_error("Unclassified collision geometry"); + return objects; +} + +QJsonObject configureMasks(SSLWorld& world, const std::vector& objects, bool enable) +{ + std::array allowed{}; + unsigned registered = 0, rejected = 0, unsupportedPassing = 0; + world.p->setCollisionFiltering(enable); + for (const auto& entry : objects) { + if (enable && dGeomGetCategoryBits(entry.object->geom) != (1UL << entry.category)) + throw std::runtime_error("Production collision category is missing or incorrect"); + allowed[entry.category] = dGeomGetCollideBits(entry.object->geom); + } + // Exhaust the actual object-pair graph, not just the eight-class table. + for (size_t i=0; igeom, second = objects[j].object->geom; + const bool passes = (dGeomGetCategoryBits(first)&dGeomGetCollideBits(second)) || + (dGeomGetCategoryBits(second)&dGeomGetCollideBits(first)); + const bool surface = world.p->findSurface(objects[i].object, objects[j].object); + if (surface) { + ++registered; + if (!passes) throw std::runtime_error("Mask removed an allowed surface pair"); + } else if (passes) ++unsupportedPassing; + else ++rejected; + } + QJsonArray classMasks; + for (auto mask : allowed) classMasks.append(int(mask)); + return {{"objects",int(objects.size())}, {"registered_pairs",int(registered)}, + {"unsupported_pairs_rejected",int(rejected)}, + {"unsupported_pairs_passing_masks",int(unsupportedPassing)}, + {"class_collide_masks",classMasks}}; +} + +void verifyBroadPhase(const std::vector& objects) +{ + // Probe the actual ODE mask path for every object pair. Overlapping spheres + // make the outcome depend only on filtering, not on fixture placement. + auto space=dSimpleSpaceCreate(nullptr); + dSpaceSetManualCleanup(space,1); + auto first=dCreateSphere(space,1), second=dCreateSphere(space,1); + unsigned callbacks=0; + auto callback=[](void* context,dGeomID,dGeomID) { ++*static_cast(context); }; + for (size_t i=0; igeom, b=objects[j].object->geom; + dGeomSetCategoryBits(first,dGeomGetCategoryBits(a)); + dGeomSetCollideBits(first,dGeomGetCollideBits(a)); + dGeomSetCategoryBits(second,dGeomGetCategoryBits(b)); + dGeomSetCollideBits(second,dGeomGetCollideBits(b)); + const bool expected=(dGeomGetCategoryBits(a)&dGeomGetCollideBits(b)) || + (dGeomGetCategoryBits(b)&dGeomGetCollideBits(a)); + callbacks=0; dSpaceCollide(space,&callbacks,callback); + if (callbacks!=unsigned(expected)) { + dSpaceDestroy(space); + throw std::runtime_error("ODE broad-phase mask probe disagrees with object pair graph"); + } + } + // A negative control demonstrates that this probe reaches mask rejection. + dGeomSetCollideBits(first,0); dGeomSetCollideBits(second,0); + callbacks=0; dSpaceCollide(space,&callbacks,callback); + dSpaceDestroy(space); + if (callbacks) throw std::runtime_error("ODE mask rejection negative control failed"); +} + +void hashBytes(uint64_t& hash, const void* data, size_t count) +{ + const auto* bytes = static_cast(data); + for (size_t i=0; i(dGeomGetData(id)); + hashBytes(hash, &index, sizeof(index)); + } + hashReal(hash, geom.depth); + for (int i=0; i<3; ++i) { hashReal(hash,geom.pos[i]); hashReal(hash,geom.normal[i]); } + const auto& s = contact.surface; + hashBytes(hash,&s.mode,sizeof(s.mode)); + for (double value : {s.mu,s.mu2,s.rho,s.rho2,s.rhoN,s.bounce,s.bounce_vel, + s.soft_erp,s.soft_cfm,s.motion1,s.motion2,s.motionN,s.slip1,s.slip2}) + hashReal(hash,value); + if (s.mode & dContactFDir1) for (int i=0; i<3; ++i) hashReal(hash,contact.fdir1[i]); +} + +struct Diagnostics { + std::vector bodies; + std::map index; + double maxAnchor = 0, anchorSquares = 0, maxAxisDegrees = 0; + double maxSpeed = 0, maxSpin = 0, minChassisZ = 1e9, maxRobotOverlap = 0; + uint64_t anchorSamples = 0, islandObservations = 0; + size_t maxIsland = 0; + double sumLargestIsland = 0, sumIslandCount = 0; + std::ofstream trace; + explicit Diagnostics(const std::vector& objects, const QString& filename) + { + for (const auto& entry : objects) if (entry.object->body) { + index[entry.object->body] = bodies.size(); + bodies.push_back(entry.object->body); + } + if (!filename.isEmpty()) { + trace.open(filename.toStdString(), std::ios::binary); + if (!trace) throw std::runtime_error("Cannot create trace file"); + } + } + void observeIslands() + { + std::vector parent(bodies.size()), size(bodies.size(),1); + std::iota(parent.begin(),parent.end(),0); + auto root = [&](size_t i) { while (parent[i]!=i) i=parent[i]; return i; }; + for (size_t i=0; isecond), b=root(second->second); + if (a!=b) { parent[b]=a; size[a]+=size[b]; } + } + } + size_t count=0, largest=0; + for (size_t i=0; i(values),sizeof(values)); + maxSpeed=std::max(maxSpeed,std::sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2])); + maxSpin=std::max(maxSpin,std::sqrt(w[0]*w[0]+w[1]*w[1]+w[2]*w[2])); + } + if (trace.is_open() && !trace) throw std::runtime_error("Failed to write body trace"); + } + void observe(SSLWorld& world) + { + state(); + for (int i=0; iRobots_Count()*2; ++i) { + minChassisZ=std::min(minChassisZ,double(dBodyGetPosition(world.robots[i]->chassis->body)[2])); + const auto* a=dBodyGetPosition(world.robots[i]->chassis->body); + for (int j=i+1; jRobots_Count()*2; ++j) { + const auto* b=dBodyGetPosition(world.robots[j]->chassis->body); + const double radii=world.robots[i]->settings.RobotRadius+world.robots[j]->settings.RobotRadius; + // Planar shell overlap for these ground-level match fixtures. + maxRobotOverlap=std::max(maxRobotOverlap,radii-std::hypot(a[0]-b[0],a[1]-b[1])); + } + for (auto* wheel : world.robots[i]->wheels) { + dVector3 a,b,axis; + dJointGetHingeAnchor(wheel->joint,a); dJointGetHingeAnchor2(wheel->joint,b); + double squared=0; + for (int j=0; j<3; ++j) squared+=(a[j]-b[j])*(a[j]-b[j]); + maxAnchor=std::max(maxAnchor,std::sqrt(squared)); anchorSquares+=squared; ++anchorSamples; + dJointGetHingeAxis(wheel->joint,axis); + const auto* rotation=dBodyGetRotation(wheel->cyl->body); + const double dot=std::abs(axis[0]*rotation[2]+axis[1]*rotation[6]+axis[2]*rotation[10]); + maxAxisDegrees=std::max(maxAxisDegrees,std::acos(std::min(1.0,dot))*180/3.141592653589793); + } + } + } + QJsonObject json() const + { + return {{"body_count",int(bodies.size())}, {"state_values_per_body",13}, + {"max_hinge_anchor_error_m",maxAnchor}, {"max_planar_robot_overlap_m",maxRobotOverlap}, + {"rms_hinge_anchor_error_m",anchorSamples ? std::sqrt(anchorSquares/anchorSamples) : 0}, + {"max_wheel_axis_error_deg",maxAxisDegrees}, {"max_linear_speed_m_s",maxSpeed}, + {"max_angular_speed_rad_s",maxSpin}, {"min_chassis_z_m",bodies.size()>1 ? minChassisZ : 0}, + {"max_island_bodies",int(maxIsland)}, + {"mean_largest_island_bodies",islandObservations ? sumLargestIsland/islandObservations : 0}, + {"mean_island_count",islandObservations ? sumIslandCount/islandObservations : 0}}; + } +}; + +QJsonObject run(SimulationSettings settings, const Options& options) +{ + const bool ballCase=options.scenario.startsWith("ball-"); + const bool skill=options.scenario=="dribble-flat" || options.scenario=="dribble-chip"; + const bool pushing=options.scenario=="pushing-6"; + const bool crowded=pushing || options.scenario.startsWith("crowded-"); + const bool group=options.scenario.startsWith("group-"); + const int robots=ballCase ? 0 : skill ? 1 : group ? options.scenario.section('-',1).toInt() : + options.scenario.endsWith("16") ? 16 : options.scenario.endsWith("-6") ? 6 : 11; + settings.value_Robots_Count=robots; settings.value_DeltaTime=tick; + RobotsFormation formation(FORMATION_INSIDE_1,&settings); + SSLWorld world(nullptr,&settings,&formation,&formation); + auto configureThreads=[&]() { + if (!world.p->setThreadCount(options.helpers)) throw std::runtime_error("Requested helpers unavailable"); + dWorldSteppingThreadingParameters threading={}; + threading.param_set=dWSTP_WorldIslandsIterationMaxThreads|dWSTP_IslandSteppingMaxThreads|dWSTP_LCPSolvingMaxThreads; + threading.world_islands_iteration_max_threads=options.islands; + threading.island_stepping_max_threads=options.preparation; + threading.lcp_solving_max_threads=1; + dWorldSetSteppingThreadingParameters(world.p->world,&threading); + }; + // Equal-input replays prepare their snapshot with the serial reference. + if (!options.replay) configureThreads(); + const auto objects=objectsOf(world); + const bool defaultFiltering=world.p->collisionFilteringEnabled(); + const auto maskAudit=configureMasks(world,objects,options.masks); + if (options.audit) verifyBroadPhase(objects); + Diagnostics diagnostics(objects,options.traceFile); + // Resolve first-step team headings before capturing any fixed link pose. + for (int i=0; istep(); + if (crowded || group) for (int i=0; isetXY((column-1.5)*0.185,(row-1)*0.185); + if (pushing) world.robots[i]->setDir(team ? 180 : 0); + } else { + world.robots[i]->setXY((i%4)*(group ? 0.35 : 0.185)-0.6, + (i/4)*(group ? 0.35 : 0.185)-0.6); + } + } + if (skill) { + world.robots[0]->setXY(0,0); world.robots[1]->setXY(3,3); + } + std::vector links; + if (group) for (int i=1; iworld,nullptr); + dJointAttach(joint,world.robots[i-1]->chassis->body,world.robots[i]->chassis->body); + dJointSetFixed(joint); links.push_back(joint); + } + dBodySetPosition(world.ball->body,4,-3,settings.BallRadius()); + PhysicsStepOptions physics; + for (int i=0; ibody,-2,0,options.scenario=="ball-flight" ? 1 : radius); + if (options.scenario=="ball-flight") dBodySetLinearVel(world.ball->body,3,0,4); + if (options.scenario=="ball-rolling") { + dBodySetLinearVel(world.ball->body,2,0,0); dBodySetAngularVel(world.ball->body,0,2/radius,0); + } + if (options.scenario=="ball-spin") dBodySetAngularVel(world.ball->body,0,0,100); + } + if (skill) { + auto* robot=world.robots[0]; + dReal dx,dy,dz,x,y,z; + robot->chassis->getBodyDirection(dx,dy,dz); + robot->kicker->box->getBodyPosition(x,y,z); + const double offset=robot->settings.KickerThickness/2+radius+0.001; + dBodySetPosition(world.ball->body,x+dx*offset,y+dy*offset,radius); + dBodySetLinearVel(world.ball->body,0,0,0); dBodySetAngularVel(world.ball->body,0,0,0); + if (!robot->kicker->isTouchingBall()) throw std::runtime_error("Dribble fixture misses kicker"); + robot->kicker->setRoller(1); robot->kicker->step(); + if (dBodyGetNumJoints(world.ball->body)!=1) throw std::runtime_error("Dribble hold not created"); + } + auto command=[&](int frame) { + for (int i=0; isetSpeed(idle ? 0 : pushing ? 0.5 : skill ? 0.3 : group && i>=robots ? -0.15 : 0.15, + idle || skill || group || pushing ? 0 : (i%2 ? 1 : -1)*0.05, + idle || skill || group || pushing ? 0 : 0.1); + } + }; + struct FrozenBody { + dBodyID body; + std::array position, velocity, spin, force, torque; + std::array quaternion; + }; + std::vector frozen; + int replayPreparationTicks=0; + if (options.replay) { + replayPreparationTicks=group ? 2 : 120; + for (int i=0; istep(); + for (auto body : diagnostics.bodies) { + dBodySetAutoDisableFlag(body,0); dBodyEnable(body); + FrozenBody state; state.body=body; + std::copy_n(dBodyGetPosition(body),3,state.position.begin()); + std::copy_n(dBodyGetQuaternion(body),4,state.quaternion.begin()); + std::copy_n(dBodyGetLinearVel(body),3,state.velocity.begin()); + std::copy_n(dBodyGetAngularVel(body),3,state.spin.begin()); + std::copy_n(dBodyGetForce(body),3,state.force.begin()); + std::copy_n(dBodyGetTorque(body),3,state.torque.begin()); + frozen.push_back(state); + } + configureThreads(); + } + PWorld::StepStatistics statistics; + statistics.measureTimings=options.profile; + uint64_t hash=UINT64_C(14695981039346656037); + if (options.audit) { + statistics.contactObserver=[&](const dContact& contact) { hashContact(hash,contact); }; + statistics.beforeSolve=[&]() { diagnostics.observeIslands(); }; + } + QJsonArray timings,contactHashes,contactCounts,ballTrajectory; + diagnostics.state(); + std::ofstream perfControl; + std::ifstream perfAck; + auto perfCommand=[&](const char* command) { + perfControl << command << '\n' << std::flush; + std::string acknowledgement; + if (!perfControl || !std::getline(perfAck,acknowledgement) || acknowledgement!="ack") + throw std::runtime_error("perf control acknowledgement failed"); + }; + if (!options.perfControl.isEmpty()) { + perfControl.open(options.perfControl.toStdString()); + perfAck.open(options.perfAck.toStdString()); + perfCommand("enable"); + } + double elapsed=0; + for (int frame=0; framespace); + for (const auto& s : frozen) { + dBodySetPosition(s.body,s.position[0],s.position[1],s.position[2]); + dBodySetQuaternion(s.body,s.quaternion.data()); + dBodySetLinearVel(s.body,s.velocity[0],s.velocity[1],s.velocity[2]); + dBodySetAngularVel(s.body,s.spin[0],s.spin[1],s.spin[2]); + dBodySetForce(s.body,s.force[0],s.force[1],s.force[2]); + dBodySetTorque(s.body,s.torque[0],s.torque[1],s.torque[2]); + } + hash=UINT64_C(14695981039346656037); + } else { + command(frame); + } + if (skill && frame==30) { + auto* kicker=world.robots[0]->kicker; + if (!kicker->isTouchingBall() || dBodyGetNumJoints(world.ball->body)!=1) + throw std::runtime_error("Dribble lost hold before kick"); + kicker->kick(4,options.scenario=="dribble-chip" ? 3 : 0); + if (dBodyGetNumJoints(world.ball->body)!=0) throw std::runtime_error("Kick did not release hold"); + kicker->setRoller(0); + } + const unsigned contactsBefore=statistics.createdContacts; + const auto start=Clock::now(); + auto* probes=options.profile || options.audit ? &statistics : nullptr; + if (options.replay) { + if (!world.p->step(tick/5,0,probes)) throw std::runtime_error("Frozen ODE substep failed"); + } else world.stepPhysics(tick,physics,probes); + const double ms=std::chrono::duration(Clock::now()-start).count(); + elapsed+=ms; timings.append(ms); + if (options.audit) { + diagnostics.observe(world); + contactHashes.append(QString::number(hash,16)); + contactCounts.append(int(statistics.createdContacts-contactsBefore)); + QJsonArray row; + for (int i=0; i<3; ++i) row.append(dBodyGetPosition(world.ball->body)[i]); + for (int i=0; i<3; ++i) row.append(dBodyGetLinearVel(world.ball->body)[i]); + for (int i=0; i<3; ++i) row.append(dBodyGetAngularVel(world.ball->body)[i]); + ballTrajectory.append(row); + } + } + // No later physics depends on the disable acknowledgement. Flush before + // serializing results so only a small stop-command boundary can be sampled. + if (perfControl.is_open()) perfControl << "disable\n" << std::flush; + for (auto link : links) dJointDestroy(link); +#ifdef GRSIM_ODE_USE_BLAS + const char* triangularBackend = grsim_ode::triangularBackend(); +#else + const char* triangularBackend = "ODE"; +#endif + QJsonObject output{{"collision_model","chassis-hull-2026"}, {"math_backend",GRSIM_ODE_MATH_BACKEND}, + {"triangular_backend",triangularBackend}, {"ode_version",QString::fromLatin1(dODE_VERSION)}, + {"configuration",QString::fromLatin1(dGetConfiguration())}, + {"scenario",options.scenario}, {"robots_per_team",robots}, {"masks",options.masks}, + {"default_filtering_enabled",defaultFiltering}, + {"helpers",int(options.helpers)}, {"island_limit",int(options.islands)}, + {"preparation_limit",int(options.preparation)}, {"lcp_limit",1}, + {"steps",options.steps}, {"warmup_ticks",options.warmup}, {"substeps",options.replay ? 1 : 5}, + {"replay",options.replay}, {"replay_preparation_ticks",replayPreparationTicks}, + {"seed",166}, {"profile",options.profile}, {"audit",options.audit}, + {"ms_per_tick",elapsed/options.steps}, {"tick_ms",timings}, {"mask_audit",maskAudit}, + {"callbacks",double(statistics.callbacks)}, {"narrow_phase_calls",double(statistics.narrowPhaseCalls)}, + {"detected_contacts",double(statistics.contacts)}, {"created_contacts",double(statistics.createdContacts)}, + {"max_penetration_m",statistics.maxPenetration}, + {"collision_ms_per_tick",1000*statistics.collisionSeconds/options.steps}, + {"narrow_phase_ms_per_tick",1000*statistics.narrowPhaseSeconds/options.steps}, + {"contact_creation_ms_per_tick",1000*statistics.contactCreationSeconds/options.steps}, + {"solver_ms_per_tick",1000*statistics.solverSeconds/options.steps}}; + if (options.audit) { + output["diagnostics"]=diagnostics.json(); output["contact_hashes"]=contactHashes; + output["contact_counts"]=contactCounts; output["ball_trajectory"]=ballTrajectory; + output["trace_file"]=options.traceFile; + } + return output; +} +} // namespace + +int main(int argc,char** argv) +{ + QApplication app(argc,argv); + Options options; + for (int i=1; i=argc) return 2; + const QString key=QString::fromLocal8Bit(argv[i]), value=QString::fromLocal8Bit(argv[++i]); + if (key=="--scenario") options.scenario=value; + else if (key=="--trace-file") options.traceFile=value; + else if (key=="--perf-control") options.perfControl=value; + else if (key=="--perf-ack") options.perfAck=value; + else { + bool valid=false; + const unsigned number=value.toUInt(&valid); + if (!valid) return 2; + if (key=="--helpers") options.helpers=number; + else if (key=="--islands") options.islands=number; + else if (key=="--prepare") options.preparation=number; + else if (key=="--steps") { if (number>10000) return 2; options.steps=int(number); } + else if (key=="--warmup") { if (number>10000) return 2; options.warmup=int(number); } + else if (key=="--masks") options.masks=number!=0; + else if (key=="--profile") options.profile=number!=0; + else if (key=="--audit") options.audit=number!=0; + else if (key=="--replay") options.replay=number!=0; + else return 2; + } + } + const QStringList scenarios={"spaced-6","crowded-6","pushing-6","spaced-11","spaced-16","crowded-11","crowded-16", + "idle-11","stopstart-11","group-4","group-8","group-11","group-16","ball-flight","ball-rolling", + "ball-spin","dribble-flat","dribble-chip"}; + if (!scenarios.contains(options.scenario) || options.helpers>16 || !options.islands || + options.islands>16 || !options.preparation || options.preparation>16 || options.steps<1 || + (!options.traceFile.isEmpty() && !options.audit) || + (options.replay && !options.scenario.contains('-')) || + (options.replay && (options.scenario.startsWith("ball-") || options.scenario.startsWith("dribble-"))) || + (options.perfControl.isEmpty()!=options.perfAck.isEmpty())) return 2; + ConfigWidget config(false); + if (options.scenario.endsWith("-6")) config.v_Division->setString("Division B"); + auto settings=config.simulationSettings(); + if (!dInitODE2(dInitFlagManualThreadCleanup) || !dAllocateODEDataForThread(dAllocateMaskAll)) return 2; + const auto previousMessageHandler=dGetMessageHandler(); + dSetMessageHandler(recordOdeMessage); + int status=0; + QJsonObject output; + try { + dRandSetSeed(166); + output=run(settings,options); output["success"]=true; + } catch (const std::exception& error) { + output["success"]=false; output["error"]=QString::fromLocal8Bit(error.what()); status=1; + } + output["lcp_errors"]=int(lcpErrors.load()); + if (lcpErrors != 0) { + output["success"]=false; output["error"]="ODE constraint solver reported failures"; status=1; + } + dSetMessageHandler(previousMessageHandler); + dCleanupODEAllDataForThread(); dCloseODE(); + std::puts(QJsonDocument(output).toJson(QJsonDocument::Compact).constData()); + return status; +} diff --git a/tests/ode_regression.cpp b/tests/ode_regression.cpp new file mode 100644 index 00000000..9be3fbc8 --- /dev/null +++ b/tests/ode_regression.cpp @@ -0,0 +1,164 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "configwidget.h" +#include "sslworld.h" + +struct Result { + std::vector state; + double milliseconds; +}; + +// Measure collision detection, dWorldStep, and robot motors without rendering, +// packet generation, timers, or real-time throttling. Default ball-ground +// contact resistance is included; the legacy custom ball force is not applied. +static Result simulate(SimulationSettings settings, unsigned helpers, int steps, bool crowded, bool prepareSerially = false) +{ + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + if (!world.p->setThreadCount(prepareSerially ? 0 : helpers)) throw std::runtime_error("Requested ODE threading is unavailable"); + if (dSpaceGetManualCleanup(world.p->space) != 1) + throw std::runtime_error("Collision space is missing manual TLS cleanup"); + for (int i = 0; i < settings.Robots_Count() * 2; ++i) { + if (crowded) world.robots[i]->setXY((i % 4) * 0.185 - 0.28, (i / 4) * 0.185 - 0.28); + world.robots[i]->setSpeed(0.15, (i % 2 ? 1 : -1) * 0.05, 0.1); + } + auto tick = [&]() { + for (int substep = 0; substep < 5; ++substep) + if (!world.p->step(settings.DeltaTime() / 5)) throw std::runtime_error("ODE physics step failed"); + for (int i = 0; i < settings.Robots_Count() * 2; ++i) world.robots[i]->step(); + }; + for (int i = 0; i < 60; ++i) tick(); + if (prepareSerially && !world.p->setThreadCount(helpers)) + throw std::runtime_error("Requested ODE threading is unavailable"); + const auto start = std::chrono::steady_clock::now(); + for (int i = 0; i < steps; ++i) tick(); + Result result; + result.milliseconds = std::chrono::duration(std::chrono::steady_clock::now() - start).count(); + for (const auto& primitive : world.p->renderPrimitives()) { + result.state.insert(result.state.end(), primitive.position.begin(), primitive.position.end()); + result.state.insert(result.state.end(), primitive.rotation.begin(), primitive.rotation.end()); + } + auto appendVelocity = [&](dBodyID body) { + const auto* v = dBodyGetLinearVel(body); + const auto* w = dBodyGetAngularVel(body); + result.state.insert(result.state.end(), v, v + 3); + result.state.insert(result.state.end(), w, w + 3); + for (int axis = 0; axis < 3; ++axis) + if (std::abs(v[axis]) > 20 || std::abs(w[axis]) > 200) + throw std::runtime_error("Unbounded body velocity"); + }; + appendVelocity(world.ball->body); + for (int i = 0; i < settings.Robots_Count() * 2; ++i) appendVelocity(world.robots[i]->chassis->body); + for (int i = 0; i < settings.Robots_Count() * 2; ++i) { + const auto* position = dBodyGetPosition(world.robots[i]->chassis->body); + if (position[2] < -0.05 || position[2] > 1) + throw std::runtime_error("Robot escaped the ground contact"); + } + for (double value : result.state) + if (!std::isfinite(value)) throw std::runtime_error("Non-finite physics state"); + return result; +} + +static bool equivalent(const Result& expected, const Result& actual, const char* description) +{ + double error = expected.state.size() == actual.state.size() ? 0 : INFINITY; + for (size_t i = 0; i < std::min(expected.state.size(), actual.state.size()); ++i) + error = std::max(error, std::abs(expected.state[i] - actual.state[i])); + const bool passed = error < 1e-8; + std::printf("%s %s max_state_difference=%.12g\n", passed ? "PASS" : "FAIL", description, error); + return passed; +} + +int main(int argc, char** argv) +{ + QApplication app(argc, argv); + ConfigWidget config(false); + bool benchmark = false, crowded = false; + unsigned helpers = 0; + int steps = 180, robots = 11; + for (int i = 1; i < argc; ++i) { + const QString option = QString::fromLocal8Bit(argv[i]); + if (option == "--benchmark") benchmark = true; + else if (option == "--crowded") crowded = true; + else if (i + 1 < argc && (option == "--threads" || option == "--steps" || option == "--robots")) { + bool valid = false; + const int value = QString::fromLocal8Bit(argv[++i]).toInt(&valid); + if (!valid || value < 0) return 2; + if (option == "--threads") helpers = unsigned(value); + else if (option == "--steps") steps = value; + else robots = value; + } else return 2; + } + if (helpers > 16 || robots < 1 || robots > MAX_ROBOT_COUNT || steps < 1) return 2; + config.v_Robots_Count->setInt(robots); + SimulationSettings settings = config.simulationSettings(); + settings.value_DeltaTime = 1.0 / 60.0; + if (!dInitODE2(dInitFlagManualThreadCleanup)) return 2; + if (!dCheckConfiguration("ODE_double_precision") || !dAllocateODEDataForThread(dAllocateMaskAll)) return 2; + bool passed = true; + try { + if (benchmark) { + const auto result = simulate(settings, helpers, steps, crowded); + QJsonObject output; + output["ode_version"] = QString::fromLatin1(dODE_VERSION); + output["configuration"] = QString::fromLatin1(dGetConfiguration()); + output["helpers"] = int(helpers); + output["robots_per_team"] = robots; + output["steps"] = steps; + output["crowded"] = crowded; + output["wall_ms"] = result.milliseconds; + output["ms_per_tick"] = result.milliseconds / steps; + QJsonArray state; + for (double value : result.state) state.append(value); + output["state"] = state; + std::puts(QJsonDocument(output).toJson(QJsonDocument::Compact).constData()); + } else { + for (bool contacts : {false, true}) { + // Threaded long crowded trajectories diverge. Check agreement + // for one tick from identical serial preparation, then bounded states; + // later contact ordering can diverge; boundedness is not equivalence. + const int comparisonSteps = contacts ? 1 : steps; + const auto serial = simulate(settings, 0, comparisonSteps, contacts, contacts); + if (dCheckConfiguration("ODE_EXT_threading") && dCheckConfiguration("ODE_THR_builtin_impl")) { + passed = equivalent(serial, simulate(settings, 2, comparisonSteps, contacts, contacts), + contacts ? "two helpers agree for one crowded tick from identical serial preparation" : "two helpers agree in match formation") && passed; + passed = equivalent(serial, simulate(settings, 4, comparisonSteps, contacts, contacts), + contacts ? "four helpers agree for one crowded tick from identical serial preparation" : "four helpers agree in match formation") && passed; + if (contacts) { + for (unsigned count : {0U, 2U, 4U}) simulate(settings, count, steps, true); + std::puts("PASS long crowded runs retain finite, bounded body states"); + } + } else std::puts("SKIP optional ODE helper threads unavailable in this library"); + } + if (dCheckConfiguration("ODE_EXT_mt_collisions") && dCheckConfiguration("ODE_EXT_threading")) { + const auto expected = simulate(settings, 0, steps, false); + auto run = [settings, steps]() { + if (!dAllocateODEDataForThread(dAllocateMaskAll)) throw std::runtime_error("Worker TLS allocation failed"); + try { + auto result = simulate(settings, 0, steps, false); + dCleanupODEAllDataForThread(); + return result; + } catch (...) { dCleanupODEAllDataForThread(); throw; } + }; + auto first = std::async(std::launch::async, run); + auto second = std::async(std::launch::async, run); + passed = equivalent(expected, first.get(), "first concurrent world is independent") && passed; + passed = equivalent(expected, second.get(), "second concurrent world is independent") && passed; + } else std::puts("SKIP concurrent worlds require ODE collision TLS and per-world schedulers"); + } + } catch (const std::exception& error) { + std::fprintf(stderr, "FAIL %s\n", error.what()); + passed = false; + } + dCleanupODEAllDataForThread(); + dCloseODE(); + return passed ? 0 : 1; +} diff --git a/tests/physics_defaults_regression.cpp b/tests/physics_defaults_regression.cpp new file mode 100644 index 00000000..b3fcc56e --- /dev/null +++ b/tests/physics_defaults_regression.cpp @@ -0,0 +1,330 @@ +#include +#include +#include +#include +#include +#include +#include "configwidget.h" +#include "simulation.h" +#include "sslworld.h" + +namespace { +constexpr double tick = 1.0 / 60; + +void require(bool condition, const char* description) +{ + if (!condition) throw std::runtime_error(description); +} + +void close(double actual, double expected, const char* description) +{ + require(std::isfinite(actual) && std::abs(actual - expected) < 1e-12, description); +} + +void checkCylinder(PCylinder* cylinder, double scale = 1) +{ + dReal radius, length; + dGeomCylinderGetParams(cylinder->geom, &radius, &length); + dMass mass; + dBodyGetMass(cylinder->body, &mass); + cylinder->setMass(mass.mass * scale); + dBodyGetMass(cylinder->body, &mass); + const double transverse = mass.mass * (radius*radius/4 + length*length/12); + close(mass.I[0], transverse, "cylinder Ixx must match local-Z geometry"); + close(mass.I[5], transverse, "cylinder Iyy must match local-Z geometry"); + close(mass.I[10], mass.mass*radius*radius/2, "cylinder Izz must match local-Z geometry"); + for (int row = 0; row < 3; ++row) + for (int column = 0; column < 3; ++column) + if (row != column) close(mass.I[4*row+column], 0, "principal inertia must be diagonal"); +} + +void settingsMigration() +{ + QTemporaryDir directory; + require(directory.isValid(), "temporary settings directory unavailable"); + const QString path = directory.filePath("settings.xml"); + auto writeLegacy = [&](const char* values) { + QFile file(path); + require(file.open(QIODevice::WriteOnly | QIODevice::Truncate), "temporary settings file unavailable"); + const QByteArray xml = QByteArray("") + + values + ""; + require(file.write(xml) == xml.size(), "temporary settings write failed"); + }; + writeLegacy("1" + "0.004" + "0.004" + "-0.7"); + { + ConfigWidget migrated(true, path); + close(migrated.BallSlip(), 0, "legacy default slip must migrate to zero"); + close(migrated.BallLinearDamp(), 0, "legacy default linear damping must migrate to zero"); + close(migrated.BallAngularDamp(), 0, "legacy default angular damping must migrate to zero"); + close(migrated.BallRollingDeceleration(), 0.35, "older settings must acquire the new rolling default"); + close(migrated.BallModelTwoPhaseAccRoll(), -0.35, "old advertised acceleration must follow actual rolling"); + migrated.v_BallRollingDeceleration->setDouble(0.42); + migrated.v_BallSlip->setDouble(1); + migrated.v_BallLinearDamp->setDouble(0.004); + migrated.v_BallAngularDamp->setDouble(0.004); + } + { + ConfigWidget reloaded(true, path); + close(reloaded.BallSlip(), 1, "migration must not repeat after user selects slip"); + close(reloaded.BallLinearDamp(), 0.004, "migration must not repeat after user selects linear damping"); + close(reloaded.BallAngularDamp(), 0.004, "migration must not repeat after user selects angular damping"); + close(reloaded.BallRollingDeceleration(), 0.42, "custom rolling deceleration must persist"); + close(reloaded.BallModelTwoPhaseAccRoll(), -0.42, "reloaded model must match custom rolling deceleration"); + } + writeLegacy("0.02" + "0.003" + "0.008"); + { + ConfigWidget custom(true, path); + close(custom.BallSlip(), 0.02, "custom legacy slip must survive migration"); + close(custom.BallLinearDamp(), 0.003, "custom legacy linear damping must survive migration"); + close(custom.BallAngularDamp(), 0.008, "custom legacy angular damping must survive migration"); + } + QFile file(path); + require(file.open(QIODevice::ReadOnly), "read-only settings fixture unavailable"); + const auto before = file.readAll(); + file.close(); + { + ConfigWidget readOnly(false, path); + close(readOnly.BallSlip(), 0.02, "explicit read-only settings must load custom values"); + readOnly.v_BallRollingDeceleration->setDouble(0.5); + readOnly.queueSettingsEdited(); + close(readOnly.BallModelTwoPhaseAccRoll(), -0.5, "live model row must follow rolling edits"); + close(readOnly.simulationSettings().BallModelTwoPhaseAccRoll(), -0.5, "settings copy must use actual rolling acceleration"); + } + require(file.open(QIODevice::ReadOnly) && file.readAll() == before, "read-only settings load must not rewrite the file"); + std::puts("PASS persisted settings migration runs once and preserves other custom values"); +} + +void checkBallSettings(SSLWorld& world, const SimulationSettings& settings) +{ + const auto* contact = world.p->findSurface(world.ball, world.ground); + const auto& material = contact->surface; + const int base = dContactBounce | dContactApprox1 | dContactRolling | dContactMu2; + const int flags = settings.BallSlip() > 0 ? base | dContactSlip1 | dContactSlip2 : base; + require(material.mode == flags && !contact->usefdir1, "free ball contact flags must survive settings edits/release"); + close(material.mu, 1, "ground sliding traction must retain its tested value"); + close(material.mu2, 1, "ground sliding traction must be isotropic"); + const double rho = settings.Gravity() > 0 ? + 1.4*settings.BallRollingDeceleration()*settings.BallRadius()/settings.Gravity() : 0; + close(material.rho, rho, "rolling torque must produce the requested deceleration"); + close(material.rho2, rho, "rolling resistance must be isotropic"); + close(material.rhoN, 2*settings.BallFriction()*settings.BallRadius(), "spin resistance must stay independent of rolling"); + close(material.bounce, settings.BallBounce(), "ball bounce must follow settings"); + close(material.bounce_vel, settings.BallBounceVel(), "bounce threshold must follow settings"); + close(material.slip1, settings.BallSlip(), "ball slip1 must follow settings"); + close(material.slip2, settings.BallSlip(), "ball slip2 must follow settings"); + close(dBodyGetLinearDamping(world.ball->body), settings.BallLinearDamp(), "linear damping must follow settings"); + close(dBodyGetAngularDamping(world.ball->body), settings.BallAngularDamp(), "angular damping must follow settings"); + dMass mass; + dBodyGetMass(world.ball->body, &mass); + close(mass.mass, settings.BallMass(), "ball mass must follow settings"); + dVector3 gravity; + dWorldGetGravity(world.p->world, gravity); + close(gravity[2], -settings.Gravity(), "gravity must follow settings at startup and live edit"); + world.sendGeomCount = 0; + std::unique_ptr packet(world.generatePacket()); + close(packet->geometry().models().straight_two_phase().acc_roll(), -settings.BallRollingDeceleration(), + "published rolling acceleration must agree with actual physics settings"); +} + +void rollingMotion(SimulationSettings settings, double speed, unsigned substeps) +{ + settings.value_Robots_Count = 0; + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + const double radius = settings.BallRadius(), a = settings.BallRollingDeceleration(); + dBodySetPosition(world.ball->body, -2, 0, radius); + dBodySetLinearVel(world.ball->body, speed, 0, 0); + dBodySetAngularVel(world.ball->body, 0, speed/radius, 0); + PhysicsStepOptions options; + options.substeps = substeps; + double previousEnergy = 0.7*settings.BallMass()*speed*speed; + const int steps = int(std::ceil((speed/a+0.1)/tick)); + for (int i = 1; i <= steps; ++i) { + world.stepPhysics(tick, options); + const double v = dBodyGetLinearVel(world.ball->body)[0]; + const double w = dBodyGetAngularVel(world.ball->body)[1]; + const double expected = std::max(0.0, speed-a*i*tick); + require(std::abs(v-expected) < 1e-8, "pure rolling speed must follow the requested deceleration"); + require(std::abs(v-radius*w) < 1e-8, "rolling must satisfy the no-slip constraint"); + const double energy = 0.5*settings.BallMass()*(v*v+0.4*radius*radius*w*w); + require(energy <= previousEnergy+1e-12, "rolling resistance must not add kinetic energy"); + previousEnergy = energy; + } + const double distance = dBodyGetPosition(world.ball->body)[0]+2; + const double idealDistance = speed*speed/(2*a); + require(std::abs(distance-idealDistance) < speed*tick/(2*substeps)+1e-5, + "rolling range must agree with energy balance within Euler integration error"); + std::printf("PASS rolling v0=%.1f, a=%.2f, substeps=%u, range=%.6f m; energy and no-slip constraints\n", + speed, a, substeps, distance); +} + +void materialsAndInertia(SimulationSettings settings) +{ + settings.value_Robots_Count = 1; + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + require(!PhysicsStepOptions().legacyBallFriction, "production must use contact resistance"); + checkBallSettings(world, settings); + require(world.ball->tag == -1, "new world must not assign a phantom ball holder"); + auto* contact = world.p->findSurface(world.ball, world.ground); + contact->callback(contact->id1, contact->id2, contact, 1); + checkBallSettings(world, settings); + for (int i = 0; i < 2; ++i) { + checkCylinder(world.robots[i]->chassis); + checkCylinder(world.robots[i]->chassis, 1.3); + for (auto* wheel : world.robots[i]->wheels) { + checkCylinder(wheel->cyl); + checkCylinder(wheel->cyl, 0.7); + } + } + std::puts("PASS production chassis/wheel inertia at creation and mass updates"); + + world.ball->tag = 0; + require(contact->callback(contact->id1, contact->id2, contact, 1), "held contact callback failed"); + require(contact->usefdir1 && (contact->surface.mode & dContactFDir1), "held contact must use holder direction"); + world.ball->tag = -1; + contact->callback(contact->id1, contact->id2, contact, 1); + checkBallSettings(world, settings); + world.ball->tag = MAX_ROBOT_COUNT*2; + contact->callback(contact->id1, contact->id2, contact, 1); + checkBallSettings(world, settings); + std::puts("PASS free contact restored after held contact and invalid holder ignored"); +} + +// Exercise the real worker configuration entry point in its owning thread. +// No event-loop iterations occur here, so the timer cannot race the assertions. +void workerSettings(SimulationSettings settings) +{ + settings.value_Robots_Count = 1; + settings.value_CommandListenPort = settings.value_SimControlListenPort = 0; + settings.value_BlueControlListenPort = settings.value_YellowControlListenPort = 0; + SimulationWorker worker(std::make_shared()); + worker.start(settings); + auto configure = [&](const SimulationSettings& next, bool rebuild) { + SimulationSettingsEdit edit; + edit.before = edit.after = next; + edit.rebuild = rebuild; + worker.configure(edit); + }; + try { + auto* original = worker.findChild(); + require(original, "worker must own its physics world"); + checkBallSettings(*original, settings); + settings.value_BallFriction = 0.08; + settings.value_BallRollingDeceleration = 0.4; + settings.value_BallSlip = 0.002; + settings.value_BallBounce = 0.65; + settings.value_BallBounceVel = 0.15; + settings.value_BallMass = 0.05; + settings.value_Gravity = 8.5; + settings.value_BallLinearDamp = 0.0001; + settings.value_BallAngularDamp = 0.0002; + configure(settings, false); + require(worker.findChild() == original, "material edits must preserve the world"); + checkBallSettings(*original, settings); + configure(settings, true); + checkBallSettings(*worker.findChild(), settings); + settings.value_BallRadius = 0.025; + configure(settings, false); + checkBallSettings(*worker.findChild(), settings); + settings.value_BallSlip = settings.value_BallLinearDamp = settings.value_BallAngularDamp = 0; + settings.value_BallFriction = 0; + configure(settings, false); + checkBallSettings(*worker.findChild(), settings); + settings.value_BallRollingDeceleration = 0; + configure(settings, false); + checkBallSettings(*worker.findChild(), settings); + std::puts("PASS startup/live/rebuilt ball settings, radius changes and zero resistance"); + } catch (...) { worker.stop(); throw; } + worker.stop(); +} + +void rollerAndKick(SimulationSettings settings, double chipSpeed) +{ + settings.value_Robots_Count = 1; + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + auto* robot = world.robots[0]; + robot->setXY(0, 0); + world.robots[1]->setXY(3, 3); + dBodySetPosition(world.ball->body, -3, -3, settings.BallRadius()); + for (int i = 0; i < 60; ++i) world.stepPhysics(tick); + dReal dx, dy, dz, x, y, z; + robot->chassis->getBodyDirection(dx, dy, dz); + robot->kicker->box->getBodyPosition(x, y, z); + const double offset = robot->settings.KickerThickness/2 + settings.BallRadius() + 0.001; + dBodySetPosition(world.ball->body, x+dx*offset, y+dy*offset, settings.BallRadius()); + dBodySetLinearVel(world.ball->body, 0, 0, 0); + dBodySetAngularVel(world.ball->body, 0, 0, 0); + require(robot->kicker->isTouchingBall(), "roller fixture must touch the actual kicker"); + const int freeJoints = dBodyGetNumJoints(world.ball->body); + robot->kicker->setRoller(1); + robot->kicker->step(); + require(dBodyGetNumJoints(world.ball->body) == freeJoints+1, "roller must create a ball hold"); + const double chassisStartX = dBodyGetPosition(robot->chassis->body)[0]; + robot->setSpeed(0.3, 0, 0); + for (int i = 0; i < 30; ++i) world.stepPhysics(tick); + require(dBodyGetPosition(robot->chassis->body)[0] > chassisStartX+0.05, "dribble fixture must move forward"); + require(robot->kicker->isTouchingBall(), "moving roller must retain the ball for half a second"); + require(dBodyGetNumJoints(world.ball->body) == freeJoints+1, "roller hold must remain active"); + robot->kicker->setRoller(0); + robot->kicker->step(); + require(dBodyGetNumJoints(world.ball->body) == freeJoints, "roller off must release the ball"); + robot->kicker->setRoller(1); + robot->kicker->step(); + require(dBodyGetNumJoints(world.ball->body) == freeJoints+1, "roller must reacquire the ball"); + robot->kicker->kick(4, chipSpeed); + require(dBodyGetNumJoints(world.ball->body) == freeJoints, "kick must release the roller hold"); + require(robot->kicker->isKicking() == (chipSpeed > 0 ? CHIP_KICK : FLAT_KICK), "kick status must match command"); + const auto* velocity = dBodyGetLinearVel(world.ball->body); + close(velocity[2], chipSpeed, "kicker must apply requested launch height velocity"); + require(std::hypot(velocity[0], velocity[1]) > 3.5, "kicker must launch the ball forward"); + const auto* initial = dBodyGetPosition(world.ball->body); + const double startX = initial[0], startY = initial[1]; + for (int i = 0; i < 10; ++i) world.stepPhysics(tick); + const auto* position = dBodyGetPosition(world.ball->body); + require(std::hypot(position[0]-startX, position[1]-startY) > 0.3, "ball must travel away after kicking"); + if (chipSpeed > 0) require(position[2] > settings.BallRadius()+0.2, "chip must lift ball clear of ground"); + checkBallSettings(world, settings); + std::printf("PASS production roller hold/release/reacquire and %s kick\n", chipSpeed > 0 ? "chip" : "flat"); +} +} // namespace + +int main(int argc, char** argv) +{ + QApplication app(argc, argv); + ConfigWidget config(false); + auto settings = config.simulationSettings(); + if (!dInitODE2(dInitFlagManualThreadCleanup) || !dAllocateODEDataForThread(dAllocateMaskAll)) return 2; + bool passed = true; + try { + settingsMigration(); + close(settings.BallRollingDeceleration(), 0.35, "default rolling deceleration must be 0.35 m/s^2"); + close(settings.BallSlip(), 0, "default ball slip must be zero"); + close(settings.BallLinearDamp(), 0, "default linear damping must be zero"); + close(settings.BallAngularDamp(), 0, "default angular damping must be zero"); + materialsAndInertia(settings); + for (double speed : {0.5, 1.0, 2.0}) rollingMotion(settings, speed, 5); + rollingMotion(settings, 2, 30); + auto alteredBall = settings; + alteredBall.value_BallMass = 0.03; + alteredBall.value_BallRadius = 0.03; + alteredBall.value_Gravity = 4; + rollingMotion(alteredBall, 2, 5); + rollerAndKick(settings, 0); + rollerAndKick(settings, 3); + } catch (const std::exception& error) { + std::fprintf(stderr, "FAIL %s\n", error.what()); + passed = false; + } + dCleanupODEAllDataForThread(); + dCloseODE(); + try { workerSettings(settings); } + catch (const std::exception& error) { std::fprintf(stderr, "FAIL %s\n", error.what()); passed = false; } + return passed ? 0 : 1; +} diff --git a/tests/physics_experiments.cpp b/tests/physics_experiments.cpp new file mode 100644 index 00000000..835a94a5 --- /dev/null +++ b/tests/physics_experiments.cpp @@ -0,0 +1,470 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "configwidget.h" +#include "sslworld.h" + +namespace { +constexpr double tickSize = 1.0 / 60.0; +struct Variant { + QString name; + double rollingScale = 0; + bool correctInertia = false; + bool finiteWheels = false; + double wheelCfm = -1; + double layer = 0; + double correctionSpeed = -1; + unsigned iterations = 0; + bool systemDefaults = false; +}; + +Variant variantFor(const QString& name) +{ + Variant v; + v.name = name; + if (name == "system") { + v.systemDefaults = true; + v.correctInertia = true; + v.rollingScale = 1; + } + else if (name == "baseline") {} + else if (name == "ball-native-half") v.rollingScale = 0.5; + else if (name == "ball-native") v.rollingScale = 1; + else if (name == "ball-native-double") v.rollingScale = 2; + else if (name == "inertia-z") v.correctInertia = true; + else if (name == "wheel-finite") v.finiteWheels = true; + else if (name == "wheel-cfm-half") v.wheelCfm = 0.001; + else if (name == "wheel-cfm-double") v.wheelCfm = 0.004; + else if (name == "contact-layer") v.layer = 0.0001; + else if (name == "contact-correction") v.correctionSpeed = 0.5; + else if (name == "quick-20") v.iterations = 20; + else if (name == "quick-80") v.iterations = 80; + else if (name == "quick-160") v.iterations = 160; + else throw std::invalid_argument("Unknown variant"); + return v; +} + +QJsonArray vectorJson(const dReal* values, int count=3) +{ + QJsonArray result; + for (int i=0; i(surface->callbackData); + surface->callbackData = context->originalData; + const bool result = context->original(first, second, surface, count); + surface->callbackData = context; + surface->surface.soft_cfm = context->cfm; + return result; +} + +// The old callback retained its held-ball settings after tag reset. Robot cases +// start with a ground ball and the former default tag of zero, so reproducing +// this behavior is necessary even though their passive ball is far from robots. +bool historicalBallContact(dGeomID, dGeomID, PSurface* surface, int) +{ + auto* world = static_cast(surface->callbackData); + if (world->ball->tag >= 0 && world->ball->tag < world->cfg->Robots_Count()*2) { + dReal x, y, z; + world->robots[world->ball->tag]->chassis->getBodyDirection(x, y, z); + surface->fdir1[0] = x; + surface->fdir1[1] = y; + surface->fdir1[2] = surface->fdir1[3] = 0; + surface->usefdir1 = true; + surface->surface.mode = dContactMu2 | dContactFDir1 | dContactSoftCFM; + surface->surface.mu = world->cfg->BallFriction(); + surface->surface.mu2 = 0.5; + surface->surface.soft_cfm = 0.002; + } + return true; +} + +double applyVariant(SSLWorld& world, const Variant& variant, + std::vector>& contexts, + unsigned substeps, bool normalizeCompliance) +{ + double complianceScale = 1; + if (normalizeCompliance && substeps != 5) { + const double erp = dWorldGetERP(world.p->world); + const double timestepRatio = 5.0/substeps; + complianceScale = 1/(erp*timestepRatio + 1-erp); + dWorldSetERP(world.p->world, erp*timestepRatio*complianceScale); + dWorldSetCFM(world.p->world, dWorldGetCFM(world.p->world)*complianceScale); + } + // Preserve the historical experiments explicitly as production defaults + // evolve. The system variant exercises the unmodified live configuration. + if (!variant.systemDefaults) { + world.p->setGravity(9.81f); + world.ball->tag = 0; + dBodySetLinearDamping(world.ball->body, 0); + dBodySetAngularDamping(world.ball->body, 0); + for (int i=0; ispace); ++i) { + const auto geom = dSpaceGetGeom(world.p->space, i); + if (dGeomGetClass(geom) != dCylinderClass) continue; + dReal radius, length; + dGeomCylinderGetParams(geom, &radius, &length); + const auto body = dGeomGetBody(geom); + dMass mass; + dBodyGetMass(body, &mass); + dMassSetCylinderTotal(&mass, mass.mass, variant.correctInertia ? 3 : 1, radius, length); + dBodySetMass(body, &mass); + } + auto* surface = world.p->findSurface(world.ball, world.ground); + surface->callback = variant.rollingScale > 0 ? nullptr : historicalBallContact; + surface->surface = {}; + surface->surface.mode = dContactBounce | dContactApprox1; + surface->surface.mu = 1; + surface->surface.bounce = world.cfg->BallBounce(); + surface->surface.bounce_vel = world.cfg->BallBounceVel(); + if (variant.rollingScale > 0) { + surface->surface.mode |= dContactRolling | dContactMu2; + surface->surface.mu2 = surface->surface.mu; + const double rho = variant.rollingScale * 2 * world.cfg->BallFriction() * world.cfg->BallRadius(); + surface->surface.rho = surface->surface.rho2 = surface->surface.rhoN = rho; + } + } + if (variant.layer > 0) dWorldSetContactSurfaceLayer(world.p->world, variant.layer); + if (variant.correctionSpeed > 0) + dWorldSetContactMaxCorrectingVel(world.p->world, variant.correctionSpeed); + if (variant.wheelCfm > 0 || complianceScale != 1) { + for (int i=0; iRobots_Count()*2; ++i) { + for (auto* wheel : world.robots[i]->wheels) { + auto* surface = world.p->findSurface(wheel->cyl, world.ground); + const double cfm = (variant.wheelCfm > 0 ? variant.wheelCfm : 0.002)*complianceScale; + contexts.emplace_back(new WheelContact{surface->callback, surface->callbackData, cfm}); + surface->callbackData = contexts.back().get(); + surface->callback = experimentalWheelContact; + } + } + } + return complianceScale; +} + +struct Diagnostics { + double maxJoint = 0, sumJointSquared = 0, maxAxisDegrees = 0; + double maxLinearSpeed = 0, maxAngularSpeed = 0, minChassisZ = 1e9; + unsigned long jointSamples = 0; + void observe(const SSLWorld& world) + { + for (int i=0; ispace); ++i) { + const auto geom = dSpaceGetGeom(world.p->space, i); + const auto body = dGeomGetBody(geom); + if (!body) continue; + vectorJson(dBodyGetPosition(body)); + vectorJson(dBodyGetRotation(body), 12); + const auto* v = dBodyGetLinearVel(body); + const auto* w = dBodyGetAngularVel(body); + vectorJson(v); vectorJson(w); + maxLinearSpeed = std::max(maxLinearSpeed, std::sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2])); + maxAngularSpeed = std::max(maxAngularSpeed, std::sqrt(w[0]*w[0]+w[1]*w[1]+w[2]*w[2])); + } + for (int i=0; iRobots_Count()*2; ++i) { + minChassisZ = std::min(minChassisZ, double(dBodyGetPosition(world.robots[i]->chassis->body)[2])); + for (auto* wheel : world.robots[i]->wheels) { + dVector3 first, second, axis; + dJointGetHingeAnchor(wheel->joint, first); + dJointGetHingeAnchor2(wheel->joint, second); + double squared = 0; + for (int j=0; j<3; ++j) squared += (first[j]-second[j])*(first[j]-second[j]); + maxJoint = std::max(maxJoint, std::sqrt(squared)); + sumJointSquared += squared; + ++jointSamples; + dJointGetHingeAxis(wheel->joint, axis); + const auto* rotation = dBodyGetRotation(wheel->cyl->body); + const double dot = std::abs(axis[0]*rotation[2]+axis[1]*rotation[6]+axis[2]*rotation[10]); + maxAxisDegrees = std::max(maxAxisDegrees, std::acos(std::min(1.0, dot))*180/M_PI); + } + } + } +}; + +QJsonArray record(const SSLWorld& world, double time) +{ + QJsonArray row; + row.append(time); + for (double value : {dBodyGetPosition(world.ball->body)[0], dBodyGetPosition(world.ball->body)[1], + dBodyGetPosition(world.ball->body)[2], dBodyGetLinearVel(world.ball->body)[0], + dBodyGetLinearVel(world.ball->body)[1], dBodyGetLinearVel(world.ball->body)[2], + dBodyGetAngularVel(world.ball->body)[0], dBodyGetAngularVel(world.ball->body)[1], + dBodyGetAngularVel(world.ball->body)[2]}) row.append(value); + if (world.cfg->Robots_Count()) { + auto* robot = world.robots[0]; + const auto* p = dBodyGetPosition(robot->chassis->body); + const auto* r = dBodyGetRotation(robot->chassis->body); + row.append(p[0]); row.append(p[1]); row.append(p[2]); + row.append(std::atan2(r[4], r[0])); + for (int i=0; i<3; ++i) row.append(dBodyGetLinearVel(robot->chassis->body)[i]); + row.append(dBodyGetAngularVel(robot->chassis->body)[2]); + } + return row; +} + +QJsonObject torqueExperiment(const SimulationSettings& settings, const Variant& variant, + bool wheel, unsigned substeps) +{ + const double mass = wheel ? settings.blueSettings.WheelMass : settings.blueSettings.BodyMass*0.99; + const double radius = wheel ? settings.blueSettings.WheelRadius : settings.blueSettings.RobotRadius; + const double length = wheel ? settings.blueSettings.WheelThickness : settings.blueSettings.RobotHeight; + const double torque = 0.02, duration = 0.2; + const double expected[3] = {mass*(radius*radius/4+length*length/12), + mass*(radius*radius/4+length*length/12), mass*radius*radius/2}; + QJsonArray inertia, observedOmega, expectedOmega; + double maxError = 0, milliseconds = 0; + for (int axis=0; axis<3; ++axis) { + PWorld world(tickSize/substeps, 0, 0); + auto* cylinder = new PCylinder(0, 0, 1, radius, length, mass, 1, 1, 1); + world.addObject(cylinder); + world.initAllObjects(); + dMass model; + dBodyGetMass(cylinder->body, &model); + if (!variant.systemDefaults) { + dMassSetCylinderTotal(&model, mass, variant.correctInertia ? 3 : 1, radius, length); + dBodySetMass(cylinder->body, &model); + } + inertia.append(model.I[axis*4+axis]); + const auto start = std::chrono::steady_clock::now(); + for (unsigned step=0; step<12*substeps; ++step) { + dVector3 applied = {0,0,0,0}; + applied[axis] = torque; + dBodyAddTorque(cylinder->body, applied[0], applied[1], applied[2]); + if (!world.step(tickSize/substeps, variant.iterations)) throw std::runtime_error("Torque step failed"); + } + milliseconds += std::chrono::duration(std::chrono::steady_clock::now()-start).count(); + const double actual = dBodyGetAngularVel(cylinder->body)[axis]; + const double reference = torque*duration/expected[axis]; + observedOmega.append(actual); expectedOmega.append(reference); + maxError = std::max(maxError, std::abs(actual/reference-1)); + } + return {{"physics_ms", milliseconds}, {"ms_per_tick", milliseconds/36}, + {"steps",36}, {"duration_s",3*duration}, {"warmup_ticks",0}, + {"metrics", QJsonObject{{"max_relative_angular_velocity_error",maxError}, + {"inertia_xyz_kg_m2",inertia}, {"omega_xyz_rad_s",observedOmega}, + {"expected_omega_xyz_rad_s",expectedOmega}}}}; +} + +QJsonObject runScenario(SimulationSettings settings, const Variant& variant, const QString& scenario, + unsigned substeps, int requestedSteps, bool normalizeCompliance, double initialSpeed) +{ + if (scenario == "torque-chassis" || scenario == "torque-wheel") + return torqueExperiment(settings, variant, scenario == "torque-wheel", substeps); + const bool ballCase = scenario.startsWith("ball-"); + const bool performance = scenario.startsWith("perf-"); + const bool crowded = scenario == "crowded-contact" || scenario == "perf-crowded"; + const int robots = ballCase ? 0 : (performance || crowded ? (scenario == "perf-16" ? 16 : 11) : 1); + settings.value_Robots_Count = robots; + settings.value_DeltaTime = tickSize; + RobotsFormation formation(FORMATION_INSIDE_1, &settings); + SSLWorld world(nullptr, &settings, &formation, &formation); + std::vector> contexts; + const double complianceScale=applyVariant(world, variant, contexts, substeps, normalizeCompliance); + PhysicsStepOptions options; + options.substeps = substeps; + options.quickStepIterations = variant.iterations; + options.legacyBallFriction = variant.rollingScale == 0; + options.finiteWheelRotation = variant.finiteWheels; + if (crowded) { + for (int i=0; isetXY((i%4)*0.185-0.28, (i/4)*0.185-0.28); + } else if (!performance && robots) { + world.robots[0]->setXY(-2, 0); + world.robots[1]->setXY(3, 3); + } + // Establish the initial heading before the recorded command response. + for (int i=0; istep(); + if (!ballCase) dBodySetPosition(world.ball->body, 4, -3, settings.BallRadius()); + int warmup = performance ? 60 : (crowded || scenario == "ball-flight" || scenario == "ball-drop" ? 0 : 60); + for (int i=0; isetSpeed(0.15, (i%2 ? 1 : -1)*0.05, 0.1); + } else if (scenario == "robot-drive") world.robots[0]->setSpeed(1.5, 0, 0); + else if (scenario == "robot-turn") world.robots[0]->setSpeed(3.0, 0, 8.0); + }; + int steps = performance ? 300 : (crowded || scenario == "ball-flight" ? 30 : 180); + if (scenario == "ball-spin" || scenario == "ball-drop") steps = 90; + if (requestedSteps > 0) steps = requestedSteps; + const double radius = settings.BallRadius(); + if (ballCase) { + dBodySetPosition(world.ball->body, -2, 0, + scenario == "ball-flight" || scenario == "ball-drop" ? 1 : radius); + dBodySetLinearVel(world.ball->body, 0, 0, 0); + dBodySetAngularVel(world.ball->body, 0, 0, 0); + if (scenario == "ball-flight") dBodySetLinearVel(world.ball->body, 3, 0, 4); + else if (scenario == "ball-rolling" || scenario == "ball-sliding") { + dBodySetLinearVel(world.ball->body, initialSpeed, 0, 0); + if (scenario == "ball-rolling") dBodySetAngularVel(world.ball->body, 0, initialSpeed/radius, 0); + } else if (scenario == "ball-spin") dBodySetAngularVel(world.ball->body, 0, 0, 100); + } + dVector3 gravity; + dWorldGetGravity(world.p->world, gravity); + Diagnostics diagnostics; + PWorld::StepStatistics contacts; + QJsonArray trajectory; + trajectory.append(record(world, 0)); + double physicsMs = 0, stopTime = -1, firstBounceHeight = 0; + double peakBallSpeed = 0, rollingSum = 0; + unsigned rollingSamples = 0; + bool bounced = false, reboundComplete = false; + double previousVz = dBodyGetLinearVel(world.ball->body)[2]; + double previousVx = dBodyGetLinearVel(world.ball->body)[0]; + for (int frame=1; frame<=steps; ++frame) { + command(); + const auto start = std::chrono::steady_clock::now(); + world.stepPhysics(tickSize, options, &contacts); + physicsMs += std::chrono::duration(std::chrono::steady_clock::now()-start).count(); + diagnostics.observe(world); + const double time = frame*tickSize; + trajectory.append(record(world, time)); + const auto* velocity = dBodyGetLinearVel(world.ball->body); + const auto* angular = dBodyGetAngularVel(world.ball->body); + const auto* position = dBodyGetPosition(world.ball->body); + const double speed = std::hypot(velocity[0],velocity[1]); + peakBallSpeed = std::max(peakBallSpeed,speed); + const bool stopped = scenario == "ball-spin" ? std::abs(angular[2])<0.1 : speed<0.02; + if (stopped && stopTime<0 && std::abs(position[2]-radius)<0.001) stopTime=time; + if (scenario == "ball-rolling" && time>=0.2 && time<=1.0) { + rollingSum += (velocity[0]-previousVx)/tickSize; + ++rollingSamples; + } + if (scenario == "ball-drop") { + if (!bounced && previousVz<0 && velocity[2]>0) bounced=true; + if (bounced && !reboundComplete) { + firstBounceHeight=std::max(firstBounceHeight,double(position[2])); + if (previousVz>0 && velocity[2]<=0) reboundComplete=true; + } + } + previousVz=velocity[2]; previousVx=velocity[0]; + } + const auto* position = dBodyGetPosition(world.ball->body); + const double duration=steps*tickSize; + QJsonObject metrics{{"max_joint_anchor_error_m", diagnostics.maxJoint}, + {"rms_joint_anchor_error_m", diagnostics.jointSamples ? std::sqrt(diagnostics.sumJointSquared/diagnostics.jointSamples) : 0}, + {"max_wheel_axis_error_deg", diagnostics.maxAxisDegrees}, + {"max_contact_penetration_m", contacts.maxPenetration}, {"detected_contacts",double(contacts.contacts)}, + {"max_body_linear_speed_m_s", diagnostics.maxLinearSpeed}, {"max_body_angular_speed_rad_s",diagnostics.maxAngularSpeed}, + {"min_chassis_z_m",robots ? diagnostics.minChassisZ : 0}, {"ball_runout_x_m",position[0]+2}, + {"ball_stop_time_s",stopTime}, {"ball_first_rebound_height_m",firstBounceHeight}, + {"mean_rolling_acceleration_m_s2",rollingSamples ? rollingSum/rollingSamples : 0}, + {"expected_matched_rolling_acceleration_m_s2",2*settings.BallFriction()*gravity[2]/1.4}, + {"expected_native_spin_stop_s",variant.rollingScale > 0 ? 100*0.4*radius*radius/ + (variant.rollingScale*2*settings.BallFriction()*radius*(-gravity[2])) : -1}, + {"peak_ball_horizontal_speed_m_s",peakBallSpeed}}; + if (scenario == "ball-flight") { + metrics["flight_horizontal_error_m"]=std::abs(position[0]-(-2+3*duration)); + metrics["flight_vertical_error_m"]=std::abs(position[2]-(1+4*duration+0.5*gravity[2]*duration*duration)); + } + QJsonObject parameters{{"robots_per_team",robots}, {"ball_radius_m",radius}, + {"ball_friction",settings.BallFriction()}, {"ball_bounce",settings.BallBounce()}, + {"rolling_scale",variant.rollingScale}, {"rolling_rho_m",variant.rollingScale*2*settings.BallFriction()*radius}, + {"cylinder_inertia_axis",variant.correctInertia ? 3 : 1}, + {"wheel_finite_rotation",variant.finiteWheels}, {"wheel_cfm",variant.wheelCfm>0 ? variant.wheelCfm : 0.002}, + {"effective_wheel_cfm",(variant.wheelCfm>0 ? variant.wheelCfm : 0.002)*complianceScale}, + {"world_erp",dWorldGetERP(world.p->world)}, {"world_cfm",dWorldGetCFM(world.p->world)}, + {"contact_layer_m",variant.layer}, {"contact_correction_m_s",variant.correctionSpeed}, + {"quickstep_iterations",int(variant.iterations)}, {"gravity_m_s2",-gravity[2]}}; + if (variant.systemDefaults) { + const auto& surface = world.p->findSurface(world.ball, world.ground)->surface; + parameters["rolling_rho_m"] = surface.rho; + parameters["spin_rho_m"] = surface.rhoN; + parameters["ball_rolling_deceleration_m_s2"] = settings.BallRollingDeceleration(); + parameters["advertised_rolling_acceleration_m_s2"] = settings.BallModelTwoPhaseAccRoll(); + parameters["ball_linear_damping"] = dBodyGetLinearDamping(world.ball->body); + parameters["ball_angular_damping"] = dBodyGetAngularDamping(world.ball->body); + parameters["ball_slip"] = settings.BallSlip(); + parameters["ball_mass_kg"] = settings.BallMass(); + parameters["field_length_m"] = settings.Field_Length(); + parameters["field_width_m"] = settings.Field_Width(); + parameters["initial_ball_speed_m_s"] = initialSpeed; + } + return {{"parameters",parameters}, {"steps",steps}, {"warmup_ticks",warmup}, + {"duration_s",duration}, {"physics_ms",physicsMs}, {"ms_per_tick",physicsMs/steps}, + {"metrics",metrics}, {"trajectory_columns",QJsonArray{"t_s","ball_x_m","ball_y_m","ball_z_m", + "ball_vx_m_s","ball_vy_m_s","ball_vz_m_s","ball_wx_rad_s","ball_wy_rad_s","ball_wz_rad_s", + "robot_x_m","robot_y_m","robot_z_m","robot_yaw_rad","robot_vx_m_s","robot_vy_m_s","robot_vz_m_s","robot_wz_rad_s"}}, + {"trajectory",trajectory}}; +} +} // namespace + +int main(int argc,char** argv) +{ + QApplication app(argc,argv); + QString variantName="baseline", scenario="ball-flight"; + QString settingsFile; + double rollingDeceleration = -1, initialSpeed = 2; + unsigned substeps=5, seed=166; + bool normalizeCompliance=false; + int steps=0; + for (int i=1; i=argc) return 2; + const QString option=QString::fromLocal8Bit(argv[i]); + const QString value=QString::fromLocal8Bit(argv[++i]); + if (option=="--variant") variantName=value; + else if (option=="--scenario") scenario=value; + else if (option=="--settings-file") settingsFile=value; + else if (option=="--rolling-deceleration" || option=="--ball-speed") { + bool valid = false; + const double number = value.toDouble(&valid); + if (!valid || !std::isfinite(number) || number < 0) return 2; + if (option=="--rolling-deceleration") rollingDeceleration=number; + else initialSpeed=number; + } + else { + bool valid=false; + const unsigned number=value.toUInt(&valid); + if (!valid) return 2; + if (option=="--substeps") substeps=number; + else if (option=="--steps") steps=number; + else if (option=="--seed") seed=number; + else if (option=="--normalize-contact-stiffness") normalizeCompliance=number != 0; + else return 2; + } + } + const QStringList scenarios={"ball-flight","ball-rolling","ball-sliding","ball-spin","ball-drop", + "torque-chassis","torque-wheel","robot-drive","robot-turn","robot-rest","crowded-contact", + "perf-11","perf-16","perf-crowded"}; + if (!substeps || substeps>120 || !scenarios.contains(scenario)) return 2; + ConfigWidget config(false, settingsFile); + auto settings = config.simulationSettings(); + if (rollingDeceleration >= 0) settings.value_BallRollingDeceleration = rollingDeceleration; + if (!dInitODE2(dInitFlagManualThreadCleanup) || !dAllocateODEDataForThread(dAllocateMaskAll)) return 2; + int status=0; + QJsonObject output{{"collision_model","chassis-hull-2026"}, {"variant",variantName}, {"scenario",scenario}, {"seed",int(seed)}, + {"substeps",int(substeps)}, {"normalized_contact_stiffness",normalizeCompliance}, + {"ode_version",QString::fromLatin1(dODE_VERSION)}}; + try { + dRandSetSeed(seed); + const auto result=runScenario(settings, variantFor(variantName), scenario, substeps, steps, + normalizeCompliance, initialSpeed); + for (auto i=result.begin(); i!=result.end(); ++i) output[i.key()]=i.value(); + output["success"]=true; + } catch (const std::exception& error) { + output["success"]=false; + output["error"]=QString::fromLocal8Bit(error.what()); + status=1; + } + dCleanupODEAllDataForThread(); dCloseODE(); + std::puts(QJsonDocument(output).toJson(QJsonDocument::Compact).constData()); + return status; +} diff --git a/tests/rendering_regression.cpp b/tests/rendering_regression.cpp index c48eb4f3..8d9efbc4 100644 --- a/tests/rendering_regression.cpp +++ b/tests/rendering_regression.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -224,6 +225,12 @@ int main(int argc, char** argv) TestView fallback(config, view->simulation); fallback.resize(512, 512); fallback.show(); + QElapsedTimer exposed; + exposed.start(); + while ((!fallback.windowHandle() || !fallback.windowHandle()->isExposed()) && + exposed.elapsed() < 2000) { + app.processEvents(); QThread::msleep(1); + } app.processEvents(); fallback.allowBlit = false; config->v_LowSpecMode->setBool(true); diff --git a/tests/worker_regression.cpp b/tests/worker_regression.cpp index 580cd0fe..33201e39 100644 --- a/tests/worker_regression.cpp +++ b/tests/worker_regression.cpp @@ -43,10 +43,74 @@ static bool check(bool passed, const char* description) return passed; } + +static bool checkOdeWorkerPolicy() +{ + const bool hadOverride = qEnvironmentVariableIsSet("GRSIM_ODE_THREADS"); + const QByteArray originalOverride = qgetenv("GRSIM_ODE_THREADS"); + qunsetenv("GRSIM_ODE_THREADS"); + QUdpSocket sink; + if (!sink.bind(QHostAddress(QHostAddress::LocalHost), 0)) return false; + QObject::connect(&sink, &QUdpSocket::readyRead, [&]() { + while (sink.hasPendingDatagrams()) { + QByteArray bytes(int(sink.pendingDatagramSize()), 0); + sink.readDatagram(bytes.data(), bytes.size()); + } + }); + ConfigWidget config(false); + config.v_Robots_Count->setInt(6); + config.v_CommandListenPort->setInt(0); + config.v_SimControlListenPort->setInt(0); + config.v_BlueControlListenPort->setInt(0); + config.v_YellowControlListenPort->setInt(0); + config.v_VisionMulticastAddr->setString("127.0.0.1"); + config.v_VisionMulticastPort->setInt(sink.localPort()); + auto settings = config.simulationSettings(); + const int available = QThread::idealThreadCount(); + const unsigned automatic = available >= 3 ? unsigned(qMin(3, available-1)) : 0; + bool passed = true; + { + SimulationController controller(settings); + auto verify = [&](unsigned expected, int robots, quint64 oldGeneration, const char* text) { + return check(waitFor([&]() { + const auto scene = controller.latest(); + return scene && scene->generation > oldGeneration && scene->tick >= 2 && + scene->robots.size() == unsigned(2*robots) && scene->odeHelpers == expected && + std::isfinite(scene->ballPosition[0]); + }), text); + }; + passed = verify(automatic, 6, 0, "large game starts with automatic island workers") && passed; + auto rebuild = [&](int robots, unsigned expected, const char* text) { + const quint64 generation = controller.latest()->generation; + settings.value_Robots_Count = robots; + controller.configure(settings, true); + return verify(expected, robots, generation, text); + }; + passed = rebuild(1, 0, "small game releases automatic island workers") && passed; + qputenv("GRSIM_ODE_THREADS", "2"); + passed = rebuild(1, 2, "explicit worker override applies to small games") && passed; + qputenv("GRSIM_ODE_THREADS", "0"); + passed = rebuild(6, 0, "explicit zero keeps large games serial") && passed; + qputenv("GRSIM_ODE_THREADS", "invalid"); + passed = rebuild(6, 0, "invalid override retains safe serial fallback") && passed; + qunsetenv("GRSIM_ODE_THREADS"); + passed = rebuild(6, automatic, "removing override restores automatic workers") && passed; + } + if (hadOverride) qputenv("GRSIM_ODE_THREADS", originalOverride); + else qunsetenv("GRSIM_ODE_THREADS"); + passed = check(!threadViolation.load(), "worker policy rebuilds and shutdown keep Qt ownership valid") && passed; + return passed; +} + int main(int argc, char** argv) { QApplication app(argc, argv); qInstallMessageHandler(captureQtMessages); + if (argc > 1 && QByteArray(argv[1]) == "--ode-worker-policy") { + const bool passed = checkOdeWorkerPolicy(); + qInstallMessageHandler(nullptr); + return passed ? 0 : 1; + } bool passed = true; LagMonitor monitor; const double step = 1.0/60.0;