[ { "path": ".clang-format", "content": "Language: Cpp\nBasedOnStyle: Microsoft\n\nAlwaysBreakTemplateDeclarations: Yes\nBinPackParameters: false\n\nBreakBeforeBinaryOperators: NonAssignment\nBreakConstructorInitializers: BeforeComma\nBreakInheritanceList: BeforeComma\n\nConstructorInitializerIndentWidth: 0\nContinuationIndentWidth: 2\n\nIndentWidth: 2\n\nNamespaceIndentation: All\n\nPackConstructorInitializers: Never\n\nPenaltyReturnTypeOnItsOwnLine: 10\nPointerAlignment: Middle\n\nSpaceAfterTemplateKeyword: false\n\nColumnLimit: 140\n\nSortIncludes: false\n\nIndentPPDirectives: BeforeHash\n\nAlignAfterOpenBracket: AlwaysBreak\n" }, { "path": ".cmake-format.yaml", "content": "markup:\n first_comment_is_literal: true\nformat:\n line_width: 100\nparse:\n additional_commands:\n cxx_flags_by_compiler_frontend:\n pargs: 0\n kwargs:\n OUTPUT: 1\n GNU: '*'\n MSVC: '*'\n FILTER: 0\n modernize_target_link_libraries:\n pargs: 1\n kwargs:\n SCOPE: 1\n TARGETS: '*'\n LIBRARIES: '*'\n INCLUDE_DIRS: '*'\n pinocchio_target:\n pargs: 1\n kwargs:\n INTERFACE: 0\n SCALAR: 1\n LIBRARY_PUBLIC_SCOPE: 1\n SOURCES: '*'\n add_pinocchio_cpp_example:\n pargs: 1\n kwargs:\n PARSERS: 0\n CPPAD: 0\n CPPADCG: 0\n CASADI: 0\n add_pinocchio_unit_test:\n pargs: 1\n kwargs:\n HEADER_ONLY: 0\n PARSERS: 0\n EXTRA: 0\n COLLISION: 0\n PARALLEL: 0\n PARSERS_OPTIONAL: 0\n EXTRA_OPTIONAL: 0\n COLLISION_OPTIONAL: 0\n PARALLEL_OPTIONAL: 0\n PACKAGES: '*'\n install_python_files:\n pargs: 0\n kwargs:\n MODULE: 1\n FILES: '*'\n" }, { "path": ".gitignore", "content": "build*\nXcode*\n*~\n*.pyc\n**/.ipynb_checkpoints\ncoverage*\n.travis\n.vscode*\n*.orig\n.cache" }, { "path": ".gitmodules", "content": "[submodule \"cmake\"]\n\tpath = cmake\n\turl = https://github.com/jrl-umi3218/jrl-cmakemodules.git\n" }, { "path": ".pre-commit-config.yaml", "content": "ci:\n autoupdate_branch: devel\n autofix_prs: false\nrepos:\n - repo: https://github.com/pre-commit/mirrors-clang-format\n rev: v18.1.5\n hooks:\n - id: clang-format\n types_or: []\n types: [text]\n files: \\.(cpp|cxx|c|h|hpp|hxx|txx)$\n - repo: https://github.com/pre-commit/pre-commit-hooks\n rev: v4.6.0\n hooks:\n - id: check-added-large-files\n - id: check-case-conflict\n - id: check-yaml\n exclude: ^packaging/conda/\n - id: detect-private-key\n# - id: end-of-file-fixer\n - id: mixed-line-ending\n - id: check-merge-conflict\n - id: trailing-whitespace\n exclude: |\n (?x)^(\n doc/doxygen-awesome.*\n )$\n - repo: https://github.com/astral-sh/ruff-pre-commit\n rev: v0.4.4\n hooks:\n - id: ruff-format\n exclude: doc/\n - repo: https://github.com/cheshirekow/cmake-format-precommit\n rev: v0.6.13\n hooks:\n - id: cmake-format\n additional_dependencies: [pyyaml>=5.1]\n - repo: https://github.com/Lucas-C/pre-commit-hooks\n rev: v1.5.5\n hooks:\n - id: forbid-tabs\n - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt\n rev: 0.2.3\n hooks:\n - id: yamlfmt\n args: [--mapping=2, --offset=2, --sequence=4, --implicit_start]\n" }, { "path": "CMakeLists.txt", "content": "#\n# Copyright (c) 2024 INRIA\n#\n\ncmake_minimum_required(VERSION 3.10)\n\nset(PROJECT_NAME simple)\nset(PROJECT_DESCRIPTION \"The Simple Simulator: Simulation Made Simple\")\nset(PROJECT_URL \"http://github.com/simple-robotics/simple\")\nset(PROJECT_CUSTOM_HEADER_EXTENSION \"hpp\")\nset(PROJECT_USE_CMAKE_EXPORT TRUE)\nset(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE)\nset(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion)\nset(SIMPLE_PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR})\n\n# Disable -Werror on Unix for now.\nset(CXX_DISABLE_WERROR True)\nset(CMAKE_VERBOSE_MAKEFILE True)\n\n# ----------------------------------------------------\n# --- OPTIONS ---------------------------------------\n# ----------------------------------------------------\n\n# Need to be set before including base.cmake\noption(INSTALL_DOCUMENTATION \"Generate and install the documentation\" OFF)\noption(USE_PRECOMPILED_HEADERS \"Use pre-compiled headers for pinocchio and eigen.\" OFF)\n\n# Check if the submodule cmake have been initialized\nset(JRL_CMAKE_MODULES \"${CMAKE_CURRENT_LIST_DIR}/cmake\")\nif(EXISTS \"${JRL_CMAKE_MODULES}/base.cmake\")\n message(STATUS \"JRL cmakemodules found in 'cmake/' git submodule\")\nelse()\n find_package(jrl-cmakemodules QUIET CONFIG)\n if(jrl-cmakemodules_FOUND)\n get_property(\n JRL_CMAKE_MODULES\n TARGET jrl-cmakemodules::jrl-cmakemodules\n PROPERTY INTERFACE_INCLUDE_DIRECTORIES)\n message(STATUS \"JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}\")\n elseif(${CMAKE_VERSION} VERSION_LESS \"3.14.0\")\n message(\n FATAL_ERROR\n \"\\nCan't find jrl-cmakemodules. Please either:\\n\"\n \" - use git submodule: 'git submodule update --init'\\n\"\n \" - or install https://github.com/jrl-umi3218/jrl-cmakemodules\\n\"\n \" - or upgrade your CMake version to >= 3.14 to allow automatic fetching\\n\")\n else()\n message(STATUS \"JRL cmakemodules not found. Let's fetch it.\")\n include(FetchContent)\n FetchContent_Declare(\n \"jrl-cmakemodules\"\n GIT_REPOSITORY \"https://github.com/jrl-umi3218/jrl-cmakemodules.git\"\n EXCLUDE_FROM_ALL)\n FetchContent_MakeAvailable(\"jrl-cmakemodules\")\n FetchContent_GetProperties(\"jrl-cmakemodules\" SOURCE_DIR JRL_CMAKE_MODULES)\n endif()\nendif()\n\nset(DOXYGEN_USE_MATHJAX YES)\nset(DOXYGEN_USE_TEMPLATE_CSS YES)\n\nif(POLICY CMP0177)\n cmake_policy(SET CMP0177 NEW)\n set(CMAKE_POLICY_DEFAULT_CMP0177 NEW)\nendif()\ninclude(\"${JRL_CMAKE_MODULES}/base.cmake\")\n\ncompute_project_args(PROJECT_ARGS LANGUAGES CXX)\nproject(${PROJECT_NAME} ${PROJECT_ARGS})\n\ninclude(\"${JRL_CMAKE_MODULES}/tracy.cmake\")\ninclude(\"${JRL_CMAKE_MODULES}/boost.cmake\")\ninclude(\"${JRL_CMAKE_MODULES}/ide.cmake\")\ninclude(\"${JRL_CMAKE_MODULES}/apple.cmake\")\nif(APPLE) # Use the handmade approach\n if(${CMAKE_VERSION} VERSION_LESS \"3.18.0\") # Need to find the right version\n set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake/find-external/OpenMP ${CMAKE_MODULE_PATH})\n endif()\nelseif(UNIX)\n if(${CMAKE_VERSION} VERSION_EQUAL \"3.20.0\")\n set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake/find-external/OpenMP ${CMAKE_MODULE_PATH})\n endif()\nendif()\ninclude(CMakeDependentOption)\n\n# If needed, set CMake policy for APPLE systems\napply_default_apple_configuration()\nif(CMAKE_VERSION VERSION_GREATER \"3.12\")\n cmake_policy(SET CMP0074 NEW)\nendif()\n\n# Force C++ standard to be C++14 at least\ncheck_minimal_cxx_standard(14 ENFORCE)\n\noption(BUILD_BENCHMARK \"Build the benchmarks\" OFF)\noption(BUILD_PYTHON_INTERFACE \"Build the Python bindings\" ON)\noption(GENERATE_PYTHON_STUBS \"Generate the Python stubs associated to the Python library\" OFF)\noption(BUILD_WITH_COMMIT_VERSION \"Build libraries by setting specific commit version\" OFF)\noption(SIMPLE_USE_DIFFCOAL \"Use diffcoal to compute collision detection derivatives\" OFF)\n\noption(BUILD_WITH_OPENMP_SUPPORT \"Build the library with the OpenMP support\" OFF)\nif(APPLE)\n option(BUILD_WITH_ACCELERATE_SUPPORT \"Build EigenPy with the Accelerate support\" OFF)\nelse(APPLE)\n set(BUILD_WITH_ACCELERATE_SUPPORT FALSE)\nendif(APPLE)\n\noption(INITIALIZE_WITH_NAN \"Initialize Eigen entries with NaN\" OFF)\noption(CHECK_RUNTIME_MALLOC \"Check if some memory allocations are performed at runtime\" OFF)\noption(ENABLE_TEMPLATE_INSTANTIATION \"Template instantiation of the main library\" ON)\noption(SIMPLE_DOWNLOAD_TRACY \"Use FetchContent to install Tracy.\" OFF)\n\nset(CFLAGS_OPTIONS)\n\nif(INITIALIZE_WITH_NAN)\n message(STATUS \"Initialize with NaN all the Eigen entries.\")\nendif(INITIALIZE_WITH_NAN)\n\nif(CHECK_RUNTIME_MALLOC)\n message(STATUS \"Check if some memory allocations are performed at runtime.\")\nendif(CHECK_RUNTIME_MALLOC)\n\nif(ENABLE_TEMPLATE_INSTANTIATION)\n message(STATUS \"Template instantiation of the main library\")\n list(APPEND CFLAGS_OPTIONS \"-DSIMPLE_ENABLE_TEMPLATE_INSTANTIATION\")\nendif(ENABLE_TEMPLATE_INSTANTIATION)\n\nmacro(TAG_LIBRARY_VERSION target)\n set_target_properties(${target} PROPERTIES SOVERSION ${PROJECT_VERSION})\nendmacro(TAG_LIBRARY_VERSION)\n\n# ----------------------------------------------------\n# --- DEPENDENCIES -----------------------------------\n# ----------------------------------------------------\n\n# -- Eigen\nadd_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES \"eigen3 >= 3.0.5\")\nif(SIMPLE_USE_DIFFCOAL)\n add_project_dependency(diffcoal REQUIRED PKG_CONFIG_REQUIRES)\nendif(SIMPLE_USE_DIFFCOAL)\n\n# -- boost\nset(BOOST_REQUIRED_COMPONENTS filesystem serialization system)\nset_boost_default_options()\nexport_boost_default_options()\nadd_project_dependency(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS})\n\n# -- pinocchio\nadd_project_dependency(pinocchio REQUIRED PKG_CONFIG_REQUIRES \"pinocchio >= 2.9.2\")\n\n# -- tracy (optional)\nif(SIMPLE_TRACY_ENABLE AND SIMPLE_DOWNLOAD_TRACY)\n # We use FetchContent_Populate because we need EXCLUDE_FROM_ALL to avoid installing Tracy with\n # simple. We can directly use EXCLUDE_FROM_ALL in FetchContent_Declare when CMake minimum version\n # will be 3.28.\n if(POLICY CMP0169)\n cmake_policy(SET CMP0169 OLD)\n endif()\n FetchContent_Declare(\n tracy\n GIT_REPOSITORY https://github.com/Simple-Robotics/tracy.git\n GIT_TAG patches\n GIT_SHALLOW TRUE\n GIT_PROGRESS TRUE)\n FetchContent_GetProperties(tracy)\n if(NOT tracy_POPULATED)\n FetchContent_Populate(tracy)\n set(TRACY_STATIC\n ON\n CACHE INTERNAL \"\")\n set(TRACY_ENABLE\n ${SIMPLE_TRACY_ENABLE}\n CACHE INTERNAL \"\")\n add_subdirectory(${tracy_SOURCE_DIR} ${tracy_BINARY_DIR} EXCLUDE_FROM_ALL)\n # Extract the target include directories, set as system\n get_target_property(tracy_INCLUDE_DIR TracyClient INTERFACE_INCLUDE_DIRECTORIES)\n set_target_properties(\n TracyClient PROPERTIES POSITION_INDEPENDENT_CODE True INTERFACE_SYSTEM_INCLUDE_DIRECTORIES\n \"${tracy_INCLUDE_DIR}\")\n endif()\nelseif(SIMPLE_TRACY_ENABLE)\n # assume it is installed somewhere\n add_project_dependency(Tracy REQUIRED)\n if(NOT ${tracy_FOUND})\n message(\n FATAL_ERROR \"Simple support for tracy is enabled, but tracy was not found on your system.\"\n \" Install it, or set the option SIMPLE_DOWNLOAD_TRACY to ON so we can fetch it.\")\n else()\n message(STATUS \"Tracy found on your system at ${Tracy_DIR}\")\n endif()\nendif()\n\n# -- python\nif(BUILD_PYTHON_INTERFACE)\n message(\n STATUS\n \"The Python bindings of Simple will be compiled along the main library. If you want to disable this feature, please set the option BUILD_PYTHON_INTERFACE to OFF.\"\n )\n\n set(PYTHON_COMPONENTS Interpreter Development NumPy)\n add_project_dependency(eigenpy 2.7.10 REQUIRED)\n\n # Check wether this a PyPy Python\n execute_process(\n COMMAND ${PYTHON_EXECUTABLE} -c \"import platform; print(platform.python_implementation())\"\n OUTPUT_VARIABLE _python_implementation_value\n OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET)\n\n message(STATUS \"Python compiler: ${_python_implementation_value}\")\n if(_python_implementation_value MATCHES \"PyPy\")\n set(BUILD_PYTHON_INTERFACE_WITH_PYPY TRUE)\n endif()\n\nelse(BUILD_PYTHON_INTERFACE)\n message(\n STATUS\n \"Simple won't be compiled with its Python bindings. If you want to enable this feature, please set the option BUILD_PYTHON_INTERFACE to ON.\"\n )\nendif(BUILD_PYTHON_INTERFACE)\n\nif(BUILD_WITH_OPENMP_SUPPORT)\n add_project_dependency(OpenMP REQUIRED)\nendif()\n\nif(BUILD_WITH_ACCELERATE_SUPPORT)\n if(NOT ${Eigen3_VERSION} VERSION_GREATER_EQUAL \"3.4.90\")\n message(\n FATAL_ERROR\n \"Your version of Eigen is too low. Should be at least 3.4.90. Current version is ${Eigen3_VERSION}.\"\n )\n endif()\n\n set(CMAKE_MODULE_PATH ${JRL_CMAKE_MODULES}/find-external/Accelerate ${CMAKE_MODULE_PATH})\n # FIND_EXTERNAL \"Accelerate\".\n #\n # We don't export yet as there might be an issue on AMR64 platforms.\n find_package(Accelerate REQUIRED)\n message(STATUS \"Build with Accelerate support framework.\")\n add_definitions(-DPINOCCHIO_WITH_ACCELERATE_SUPPORT -DSIMPLE_WITH_ACCELERATE_SUPPORT)\nendif(BUILD_WITH_ACCELERATE_SUPPORT)\n\n# ----------------------------------------------------\n# --- MAIN LIBRARY -----------------------------------\n# ----------------------------------------------------\n# Sources definition\ninclude(sources.cmake)\n\n# List headers to install\nlist(APPEND ${PROJECT_NAME}_HEADERS ${${PROJECT_NAME}_CORE_PUBLIC_HEADERS})\n\nif(ENABLE_TEMPLATE_INSTANTIATION)\n list(APPEND ${PROJECT_NAME}_CORE_SOURCES ${${PROJECT_NAME}_TEMPLATE_INSTANTIATION_SOURCES})\n list(APPEND ${PROJECT_NAME}_CORE_PUBLIC_HEADERS\n ${${PROJECT_NAME}_TEMPLATE_INSTANTIATION_PUBLIC_HEADERS})\nendif()\n\nif(BUILD_PYTHON_INTERFACE)\n list(APPEND ${PROJECT_NAME}_HEADERS ${${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS})\nendif()\nadd_subdirectory(src)\n\n# ----------------------------------------------------\n# --- BENCHMARK --------------------------------------\n# ----------------------------------------------------\nif(BUILD_BENCHMARK)\n add_subdirectory(benchmark)\nendif(BUILD_BENCHMARK)\n\n# ----------------------------------------------------\n# --- BINDINGS ---------------------------------------\n# ----------------------------------------------------\nadd_subdirectory(bindings)\n\n# ----------------------------------------------------\n# --- UNIT TESTS -------------------------------------\n# ----------------------------------------------------\n# test data\nconfig_files(tests/test_data/config.h)\nadd_subdirectory(tests)\n\n# Export for PkgConfig\npkg_config_append_libs(${PROJECT_NAME})\npkg_config_append_boost_libs(${BOOST_COMPONENTS})\n" }, { "path": "LICENSE", "content": "BSD 3-Clause License\n\nCopyright (c) 2025, INRIA\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n contributors may be used to endorse or promote products derived from\n this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "README.md", "content": "# The Simple Simulator: Simulation Made Simple\n\n**Simple** is a new (differentiable) physical engine based on recent progress on solving contact simulation and leveraging [Pinocchio](https://github.com/stack-of-tasks/pinocchio) for fast dynamics computations and [Coal](https://github.com/coal-library/coal/) for efficient collision detection.\nWhile first targetting robotics applications, **Simple** can be exploited in many other contexts: video games, system design, graphical animations, biomechanics, etc.\n\n**Simple** is developed by the [WILLOW team](https://www.di.ens.fr/willow/) at [Inria](https://www.inria.fr/en).\nThe code associated with **Simple** has been released on May, 26th 2025 under the permissive BSD-3 license.\n\nMore features and improved efficiency will come soon. And as Jean de La Fontaine wrote: **\"Patience and time do more than strength or passion\"**.\n\n\n \n## The core team\n\nThe following persons actively took part in the development of **Simple**:\n- [Justin Carpentier](https://jcarpent.github.io/) (Inria): core developer and project instigator\n- [Quentin Le Lidec](https://quentinll.github.io/) (Inria): core developer\n- [Louis Montaut](https://lmontaut.github.io/) (Inria): core developer\n- [Joris Vaillant](https://github.com/jorisv/) (Inria): core developer\n- [Yann de Mont-Marin](https://github.com/ymontmarin) (Inria): core developer\n- [Ajay Sathya](https://www.ajaysathya.com/) (Inria): feature contributor\n- [Fabian Schramm](https://github.com/fabinsch) (Inria): feature contributor\n\nExternal contributions are more than welcome. If you have contributed to the development of Simple, feel free to add your name.\n\n## Associated scientific and technical publications\n\n**Simple** is built on active research around understanding and enhancing physical simulation.\nInterested readers can learn more about the algorithmic and computational foundations of **Simple** by reading these publications:\n\n- Le Lidec, Q., Montaut, L. & Carpentier, J. (2024, July). [From Compliant to Rigid Contact Simulation: a Unified and Efficient Approach](https://hal.science/hal-04588906). In RSS 2024-Robotics: Science and Systems.\n- Montaut, L., Le Lidec, Q., Petrik, V., Sivic, J., & Carpentier, J. (2024). [GJK++: Leveraging Acceleration Methods for Faster Collision Detection](https://hal.science/hal-04070039/). IEEE Transactions on Robotics.\n- Sathya, A., & Carpentier, J. (2024). [Constrained Articulated Body Dynamics Algorithms](https://hal.science/hal-04443056/). IEEE Transactions on Robotics.\n- Montaut, L., Le Lidec, Q., Bambade, A., Petrik, V., Sivic, J., & Carpentier, J. (2023, May). [Differentiable collision detection: a randomized smoothing approach](https://hal.science/hal-03780482/). In 2023 IEEE International Conference on Robotics and Automation (ICRA).\n- Le Lidec, Q., Jallet, W., Montaut, L., Laptev, I., Schmid, C., & Carpentier, J. (2023). [Contact models in robotics: a comparative analysis](https://hal.science/hal-04067291/). IEEE Transactions on Robotics.\n- Montaut, L., Le Lidec, Q., Petrik, V., Sivic, J., & Carpentier, J. (2022, June). [Collision Detection Accelerated: An Optimization Perspective](https://hal.science/hal-03662157/). In Robotics: Science and Systems (RSS 2O22).\n- Carpentier, J., Budhiraja, R., & Mansard, N. (2021, July). [Proximal and sparse resolution of constrained dynamic equations](https://hal.science/hal-03271811/). In Robotics: Science and Systems (RSS 2021).\n- Carpentier, J., & Mansard, N. (2018, June). [Analytical derivatives of rigid body dynamics algorithms](https://hal.science/hal-01790971/). In Robotics: Science and systems (RSS 2018).\n" }, { "path": "benchmark/CMakeLists.txt", "content": "add_project_private_dependency(benchmark REQUIRED)\n\nadd_custom_target(bench)\n\nfunction(ADD_SIMPLE_BENCHMARK benchmark_name)\n add_executable(${benchmark_name} ${benchmark_name}.cpp)\n target_link_libraries(${benchmark_name} PRIVATE ${PROJECT_NAME})\n target_link_libraries(${benchmark_name} PRIVATE benchmark::benchmark)\n\n add_dependencies(bench ${benchmark_name})\nendfunction()\n\nadd_simple_benchmark(mujoco-humanoid)\nadd_simple_benchmark(affine-transform)\n" }, { "path": "benchmark/affine-transform.cpp", "content": "#include \n#include \n\n#define N 100\n#define WARMUP_TIME 1.\n#define MIN_TIME 2.\n\nusing Vec3f = Eigen::Matrix;\nusing Mat3f = Eigen::Matrix;\nusing Vec4f = Eigen::Matrix;\nusing Mat4f = Eigen::Matrix;\nusing Mat34f = Eigen::Matrix;\n\nusing Vec3d = Eigen::Matrix;\nusing Mat3d = Eigen::Matrix;\nusing Vec4d = Eigen::Matrix;\nusing Mat4d = Eigen::Matrix;\nusing Mat34d = Eigen::Matrix;\n\nusing Vec3i16 = Eigen::Matrix;\nusing Mat3i16 = Eigen::Matrix;\nusing Vec4i16 = Eigen::Matrix;\nusing Mat4i16 = Eigen::Matrix;\nusing Mat34i16 = Eigen::Matrix;\n\ntemplate\nstruct BenchFixture : benchmark::Fixture\n{\n void SetUp(benchmark::State &)\n {\n y.reserve(N);\n x.reserve(N);\n t.reserve(N);\n M.reserve(N);\n dotprod.reserve(N);\n for (std::size_t i = 0; i < N; ++i)\n {\n y.push_back(ResVec::Zero());\n x.push_back(InVec::Random());\n t.push_back(InVec::Random());\n M.push_back(Mat::Random());\n dotprod.push_back(0);\n }\n }\n\n void TearDown(benchmark::State &)\n {\n }\n\n // y = sum_i M_i * x_i + t_i\n std::vector> y;\n std::vector> x;\n std::vector> t;\n std::vector> M;\n std::vector dotprod;\n};\n\nusing Bench33f = BenchFixture;\nBENCHMARK_DEFINE_F(Bench33f, Mat3Vec3f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].noalias() = M[i] * x[i] + t[i];\n }\n }\n}\nBENCHMARK_REGISTER_F(Bench33f, Mat3Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing Bench33d = BenchFixture;\nBENCHMARK_DEFINE_F(Bench33d, Mat3Vec3d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].noalias() = M[i] * x[i] + t[i];\n }\n }\n}\nBENCHMARK_REGISTER_F(Bench33d, Mat3Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing Bench44f = BenchFixture;\nBENCHMARK_DEFINE_F(Bench44f, Mat4Vec4f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].noalias() = M[i] * x[i];\n }\n }\n}\nBENCHMARK_REGISTER_F(Bench44f, Mat4Vec4f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing Bench44d = BenchFixture;\nBENCHMARK_DEFINE_F(Bench44d, Mat4Vec4d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].noalias() = M[i] * x[i];\n }\n }\n}\nBENCHMARK_REGISTER_F(Bench44d, Mat4Vec4d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing Bench34f = BenchFixture;\nBENCHMARK_DEFINE_F(Bench34f, Mat34Vec3f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].noalias() = M[i] * x[i];\n }\n }\n}\nBENCHMARK_REGISTER_F(Bench34f, Mat34Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing Bench34d = BenchFixture;\nBENCHMARK_DEFINE_F(Bench34d, Mat34Vec3d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].noalias() = M[i] * x[i];\n }\n }\n}\nBENCHMARK_REGISTER_F(Bench34d, Mat34Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing YannBench34f = BenchFixture;\nBENCHMARK_DEFINE_F(YannBench34f, Mat34Vec3f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].head<3>().noalias() = M[i] * x[i];\n y[i].coeffRef(3) = 1.0f;\n }\n }\n}\nBENCHMARK_REGISTER_F(YannBench34f, Mat34Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing YannBench34d = BenchFixture;\nBENCHMARK_DEFINE_F(YannBench34d, Mat34Vec3d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n y[i].head<3>().noalias() = M[i] * x[i];\n y[i].coeffRef(3) = 1.0f;\n }\n }\n}\nBENCHMARK_REGISTER_F(YannBench34d, Mat34Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing DotBench3f = BenchFixture;\nBENCHMARK_DEFINE_F(DotBench3f, Vec3f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n dotprod[i] = x[i].dot(t[i]);\n }\n }\n}\nBENCHMARK_REGISTER_F(DotBench3f, Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing DotBench3d = BenchFixture;\nBENCHMARK_DEFINE_F(DotBench3d, Vec3d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n dotprod[i] = x[i].dot(t[i]);\n }\n }\n}\nBENCHMARK_REGISTER_F(DotBench3d, Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing DotBench4f = BenchFixture;\nBENCHMARK_DEFINE_F(DotBench4f, Vec4f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n dotprod[i] = x[i].dot(t[i]);\n }\n }\n}\nBENCHMARK_REGISTER_F(DotBench4f, Vec4f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing DotBench4d = BenchFixture;\nBENCHMARK_DEFINE_F(DotBench4d, Vec4d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n dotprod[i] = x[i].dot(t[i]);\n }\n }\n}\nBENCHMARK_REGISTER_F(DotBench4d, Vec4d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing YannDotBench4f = BenchFixture;\nBENCHMARK_DEFINE_F(YannDotBench4f, Vec4f)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n dotprod[i] = (x[i].cwiseProduct(t[i])).head<3>().sum();\n }\n }\n}\nBENCHMARK_REGISTER_F(YannDotBench4f, Vec4f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nusing YannDotBench4d = BenchFixture;\nBENCHMARK_DEFINE_F(YannDotBench4d, Vec4d)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n for (std::size_t i = 0; i < x.size(); ++i)\n {\n dotprod[i] = (x[i].cwiseProduct(t[i])).head<3>().sum();\n }\n }\n}\nBENCHMARK_REGISTER_F(YannDotBench4d, Vec4d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME);\n\nBENCHMARK_MAIN();\n" }, { "path": "benchmark/mujoco-humanoid.cpp", "content": "//\n// Copyright (c) 2025 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \"tests/test_data/config.h\"\n\n#include \n#include \n\n#include \n\nusing namespace simple;\nusing namespace pinocchio;\nusing ModelHandle = Simulator::ModelHandle;\nusing DataHandle = Simulator::DataHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\nusing GeometryDataHandle = Simulator::GeometryDataHandle;\n#define ADMM ::pinocchio::ADMMContactSolverTpl\n#define PGS ::pinocchio::PGSContactSolverTpl\n\nconst double DT = 1e-3;\nconst std::size_t NUM_SIM_STEPS = 100;\n\nvoid addFloorToGeomModel(GeometryModel & geom_model)\n{\n using CollisionGeometryPtr = GeometryObject::CollisionGeometryPtr;\n\n CollisionGeometryPtr floor_collision_shape = CollisionGeometryPtr(new hpp::fcl::Halfspace(0.0, 0.0, 1.0, 0.0));\n\n const SE3 M = SE3::Identity();\n GeometryObject floor(\"floor\", 0, 0, M, floor_collision_shape);\n geom_model.addGeometryObject(floor);\n}\n\nvoid addSystemCollisionPairs(const Model & model, GeometryModel & geom_model, const Eigen::VectorXd & qref)\n{\n Data data(model);\n GeometryData geom_data(geom_model);\n // TI this function to gain compilation speed on this test\n ::pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, qref);\n geom_model.removeAllCollisionPairs();\n for (std::size_t i = 0; i < geom_model.geometryObjects.size(); ++i)\n {\n for (std::size_t j = i; j < geom_model.geometryObjects.size(); ++j)\n {\n if (i == j)\n {\n continue; // don't add collision pair if same object\n }\n const GeometryObject & gobj_i = geom_model.geometryObjects[i];\n const GeometryObject & gobj_j = geom_model.geometryObjects[j];\n if (gobj_i.name == \"floor\" || gobj_j.name == \"floor\")\n { // if floor, always add a collision pair\n geom_model.addCollisionPair(CollisionPair(i, j));\n }\n else\n {\n if (gobj_i.parentJoint == gobj_j.parentJoint)\n { // don't add collision pair if same parent\n continue;\n }\n\n // run collision detection -- add collision pair if shapes are not colliding\n const SE3 M1 = geom_data.oMg[i];\n const SE3 M2 = geom_data.oMg[j];\n\n hpp::fcl::CollisionRequest colreq;\n colreq.security_margin = 1e-2; // 1cm of clearance\n hpp::fcl::CollisionResult colres;\n hpp::fcl::collide(\n gobj_i.geometry.get(), ::pinocchio::toFclTransform3f(M1), //\n gobj_j.geometry.get(), ::pinocchio::toFclTransform3f(M2), //\n colreq, colres);\n if (!colres.isCollision())\n {\n geom_model.addCollisionPair(CollisionPair(i, j));\n }\n }\n }\n }\n}\n\nstruct MujocoHumanoidFixture : benchmark::Fixture\n{\n void SetUp(benchmark::State &)\n {\n ModelHandle model_handle = std::make_shared();\n GeometryModelHandle geom_model_handle = std::make_shared();\n Model & model = ::pinocchio::helper::get_ref(model_handle);\n GeometryModel & geom_model = ::pinocchio::helper::get_ref(geom_model_handle);\n\n const bool verbose = false;\n ::pinocchio::mjcf::buildModel(SIMPLE_TEST_DATA_DIR \"/mujoco_humanoid.xml\", model, verbose);\n ::pinocchio::mjcf::buildGeom(model, SIMPLE_TEST_DATA_DIR \"/mujoco_humanoid.xml\", pinocchio::COLLISION, geom_model);\n addFloorToGeomModel(geom_model);\n\n // initial state\n q0 = model.referenceConfigurations[\"qpos0\"];\n v0 = Eigen::VectorXd::Zero(model.nv);\n zero_torque = Eigen::VectorXd::Zero(model.nv);\n\n // add collision pairs\n addSystemCollisionPairs(model, geom_model, q0);\n\n sim = std::make_unique(model_handle, geom_model_handle);\n }\n\n void TearDown(benchmark::State &)\n {\n }\n\n std::unique_ptr sim;\n Eigen::VectorXd q0;\n Eigen::VectorXd v0;\n Eigen::VectorXd zero_torque;\n};\n\nBENCHMARK_DEFINE_F(MujocoHumanoidFixture, admm)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = v0;\n sim->reset();\n bool warm_start = st.range(0);\n sim->warm_start_constraints_forces = warm_start;\n for (size_t i = 0; i < NUM_SIM_STEPS; ++i)\n {\n sim->step(q, v, zero_torque, DT);\n q = sim->qnew;\n v = sim->vnew;\n }\n }\n}\nBENCHMARK_REGISTER_F(MujocoHumanoidFixture, admm)\n ->ArgName(\"warmstart\")\n ->Arg(0)\n ->Arg(1)\n ->Unit(benchmark::kMillisecond)\n ->MinWarmUpTime(3.)\n ->MinTime(5.);\n\nBENCHMARK_DEFINE_F(MujocoHumanoidFixture, pgs)(benchmark::State & st)\n{\n for (auto _ : st)\n {\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = v0;\n sim->reset();\n bool warm_start = st.range(0);\n sim->warm_start_constraints_forces = warm_start;\n for (size_t i = 0; i < NUM_SIM_STEPS; ++i)\n {\n sim->step(q, v, zero_torque, DT);\n q = sim->qnew;\n v = sim->vnew;\n }\n }\n}\nBENCHMARK_REGISTER_F(MujocoHumanoidFixture, pgs)\n ->ArgName(\"warmstart\")\n ->Arg(0)\n ->Arg(1)\n ->Unit(benchmark::kMillisecond)\n ->MinWarmUpTime(3.)\n ->MinTime(5.);\n\nBENCHMARK_MAIN();\n" }, { "path": "bindings/CMakeLists.txt", "content": "#\n# Copyright (c) 2024 INRIA\n#\n\nadd_subdirectory(python)\nif(BUILD_PYTHON_INTERFACE)\n set(PYWRAP\n ${PYWRAP}\n PARENT_SCOPE)\nendif(BUILD_PYTHON_INTERFACE)\n" }, { "path": "bindings/python/CMakeLists.txt", "content": "#\n# Copyright (c) 2024 INRIA\n#\n\ninclude(${JRL_CMAKE_MODULES}/python-helpers.cmake)\ninclude(${JRL_CMAKE_MODULES}/stubs.cmake)\n\n# --- PYTHON TARGET --- #\nset(PYWRAP ${PROJECT_NAME}_pywrap)\nset(PYWRAP\n ${PYWRAP}\n PARENT_SCOPE)\n\n# --- COMPILE WRAPPER\nmake_directory(\"${${PROJECT_NAME}_BINARY_DIR}/bindings/python/${PROJECT_NAME}\")\n\nset(${PYWRAP}_SOURCES ${${PROJECT_NAME}_BINDINGS_PYTHON_SOURCES})\nset(${PYWRAP}_HEADERS ${${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS})\n\nif(BUILD_PYTHON_INTERFACE)\n add_custom_target(python)\n set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)\n python_build_get_target(python_build_target)\n add_dependencies(python ${python_build_target})\n\n set(PKG_CONFIG_PYWRAP_REQUIRES \"eigenpy >= 2.6.5\")\n if(IS_ABSOLUTE ${PYTHON_SITELIB})\n set(ABSOLUTE_PYTHON_SITELIB ${PYTHON_SITELIB})\n else()\n set(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB})\n endif()\n set(SIMPLE_PYTHON_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME})\n\n function(INSTALL_PYTHON_FILES)\n set(options)\n set(oneValueArgs MODULE)\n set(multiValueArgs FILES)\n cmake_parse_arguments(ARGS \"${options}\" \"${oneValueArgs}\" \"${multiValueArgs}\" ${ARGN})\n\n set(SOURCE_PATH ${PROJECT_NAME})\n set(INSTALL_PATH ${SIMPLE_PYTHON_INSTALL_DIR})\n if(ARGS_MODULE)\n set(SOURCE_PATH ${SOURCE_PATH}/${ARGS_MODULE})\n set(INSTALL_PATH ${INSTALL_PATH}/${ARGS_MODULE})\n endif()\n\n foreach(f ${ARGS_FILES})\n python_build(${SOURCE_PATH} ${f})\n install(FILES ${SOURCE_PATH}/${f} DESTINATION ${INSTALL_PATH})\n endforeach()\n endfunction()\n\n # --- PYTHON LIB --- #\n set(PYTHON_LIB_NAME \"${PYWRAP}\")\n set(${PYTHON_LIB_NAME}_SOURCES ${${PYWRAP}_SOURCES})\n set(${PYTHON_LIB_NAME}_HEADERS ${${PYWRAP}_HEADERS})\n\n add_library(${PYTHON_LIB_NAME} SHARED ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS})\n\n add_dependencies(python ${PYTHON_LIB_NAME})\n set_target_properties(${PYTHON_LIB_NAME} PROPERTIES PREFIX \"\") # Remove lib prefix for the target\n\n # Do not report: -Wconversion as the BOOST_PYTHON_FUNCTION_OVERLOADS implicitly converts.\n # -Wcomment as latex equations have multi-line comments. -Wself-assign-overloaded as bp::self\n # operations trigger this\n if(NOT WIN32)\n target_compile_options(${PYTHON_LIB_NAME} PRIVATE -Wno-conversion -Wno-comment\n -Wno-self-assign-overloaded)\n endif(NOT WIN32)\n\n set_target_properties(${PYTHON_LIB_NAME} PROPERTIES VERSION ${PROJECT_VERSION})\n add_header_group(${PYTHON_LIB_NAME}_HEADERS)\n add_source_group(${PYTHON_LIB_NAME}_SOURCES)\n\n modernize_target_link_libraries(\n ${PYTHON_LIB_NAME}\n SCOPE PUBLIC\n TARGETS eigenpy::eigenpy)\n target_link_libraries(${PYTHON_LIB_NAME} PUBLIC ${PROJECT_NAME})\n target_compile_definitions(${PYTHON_LIB_NAME}\n PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_HPP_FCL_PYTHON_BINDINGS)\n if(WIN32)\n target_compile_definitions(${PYTHON_LIB_NAME} PRIVATE -DNOMINMAX)\n target_link_libraries(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY})\n endif(WIN32)\n target_compile_options(${PYTHON_LIB_NAME} PRIVATE -DSIMPLE_PYTHON_MODULE_NAME=${PYTHON_LIB_NAME})\n\n set(${PYWRAP}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME})\n\n set_target_properties(\n ${PYTHON_LIB_NAME}\n PROPERTIES PREFIX \"\"\n SUFFIX ${PYTHON_EXT_SUFFIX}\n OUTPUT_NAME \"${PYTHON_LIB_NAME}\"\n LIBRARY_OUTPUT_DIRECTORY \"${CMAKE_BINARY_DIR}/bindings/python/${PROJECT_NAME}\"\n # On Windows, shared library are treat as binary\n RUNTIME_OUTPUT_DIRECTORY \"${CMAKE_BINARY_DIR}/bindings/python/${PROJECT_NAME}\")\n\n if(UNIX AND NOT APPLE)\n get_relative_rpath(${${PYWRAP}_INSTALL_DIR} ${PYWRAP}_INSTALL_RPATH)\n set_target_properties(${PYTHON_LIB_NAME} PROPERTIES INSTALL_RPATH \"${${PYWRAP}_INSTALL_RPATH}\")\n endif()\n\n install(\n TARGETS ${PYTHON_LIB_NAME}\n EXPORT ${TARGETS_EXPORT_NAME}\n DESTINATION ${SIMPLE_PYTHON_INSTALL_DIR})\n\n install_python_files(FILES __init__.py)\n\n # --- STUBS --- #\n if(GENERATE_PYTHON_STUBS)\n python_build_get_target(python_build_target_name)\n load_stubgen()\n # Set PYWRAP and PROJECT_NAME as stubs dependencies.\n #\n # PROJECT_NAME is mandatory (even if it's a PYWRAP dependency) to find PROJECT_NAME name DLL on\n # windows.\n generate_stubs(\n ${CMAKE_CURRENT_BINARY_DIR}\n ${PROJECT_NAME}\n ${ABSOLUTE_PYTHON_SITELIB}\n ${PYWRAP}\n ${PROJECT_NAME}\n ${STUBGEN_DEPENDENCIES}\n ${python_build_target_name})\n endif(GENERATE_PYTHON_STUBS)\n\nendif(BUILD_PYTHON_INTERFACE)\n" }, { "path": "bindings/python/core/expose-constraints-problem.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/bindings/python/core/constraints-problem.hpp\"\n\nnamespace bp = boost::python;\n\nnamespace simple\n{\n namespace python\n {\n\n void exposeConstraintsProblem()\n {\n bp::class_(\n \"ContactMapper\",\n \"Maps a single collision pair to contact points information (contact position, contact \"\n \"normal, contact placements, constraint models/datas, friction cones, elasticities, \"\n \"penetration depths).\",\n bp::no_init)\n .def_readwrite(\n \"begin\", &ConstraintsProblem::ContactMapper::begin, \"The id of the first contact point for the considered collision pair.\")\n .def_readwrite(\"count\", &ConstraintsProblem::ContactMapper::count, \"Number of contact points for the considered collision pair\");\n\n using ContactMode = ConstraintsProblem::ContactMode;\n bp::enum_(\"ContactMode\")\n .value(\"BREAKING\", ContactMode::BREAKING)\n .value(\"STICKING\", ContactMode::STICKING)\n .value(\"SLIDING\", ContactMode::SLIDING)\n .export_values();\n\n // Register a handle to ConstraintsProblem\n using ConstraintsProblemHandle = std::shared_ptr;\n bp::register_ptr_to_python();\n\n ConstraintsProblemPythonVisitor::expose();\n }\n\n } // namespace python\n} // namespace simple\n" }, { "path": "bindings/python/core/expose-contact-frame.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/contact-frame.hpp\"\n#include \"simple/bindings/python/fwd.hpp\"\n\n#include \n\nnamespace bp = boost::python;\n\nnamespace simple\n{\n namespace python\n {\n using SE3 = ::simple::PlacementFromNormalAndPosition::SE3;\n using Vector3s = ::simple::PlacementFromNormalAndPosition::Vector3s;\n using Matrix6x3s = ::simple::PlacementFromNormalAndPosition::Matrix6x3s;\n\n SE3 placementFromNormalAndPosition(const Vector3s & normal, const Vector3s & position)\n {\n SE3 M;\n ::simple::PlacementFromNormalAndPosition::calc(normal, position, M);\n return M;\n }\n\n bp::tuple placementFromNormalAndPositionDerivative(const SE3 & M)\n {\n Matrix6x3s dM_dnormal;\n Matrix6x3s dM_dposition;\n ::simple::PlacementFromNormalAndPosition::calcDiff(M, dM_dnormal, dM_dposition);\n return bp::make_tuple(dM_dnormal, dM_dposition);\n }\n\n void exposeContactFrame()\n {\n bp::def(\n \"placementFromNormalAndPosition\", placementFromNormalAndPosition, bp::args(\"normal\", \"position\"),\n \"Returns a placement such that `normal` is the z-axis of the placement's rotation and \"\n \"`position` is the translation of the placement.\\n\"\n \"Parameters:\\n\"\n \"\\tnormal: z-axis of the placement's rotation.\\n\"\n \"\\tposition: translation part of the placement.\\n\\n\"\n \"Returns: M, the placement.\");\n\n bp::def(\n \"placementFromNormalAndPositionDerivative\", placementFromNormalAndPositionDerivative, bp::args(\"M\"),\n \"Returns the derivatives of a placement w.r.t both the normal and position than generated \"\n \"it. The normal is the z-axis of the placement's rotation and the position is the \"\n \"translation of the part of the placement.\"\n \"Parameters:\\n\"\n \"\\tM: a placement.\\n\\n\"\n \"Returns: a tuple (dM_dnormal, dM_dposition), both are 6x3 matrices.\");\n }\n\n } // namespace python\n} // namespace simple\n" }, { "path": "bindings/python/core/expose-simulator.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/bindings/python/core/simulator.hpp\"\n\nnamespace simple\n{\n namespace python\n {\n\n void exposeSimulator()\n {\n SimulatorPythonVisitor::expose();\n }\n\n } // namespace python\n} // namespace simple\n" }, { "path": "bindings/python/module.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/bindings/python/fwd.hpp\"\n#include \"simple/config.hpp\"\n#include \"simple/fwd.hpp\"\n\n#include \n\nnamespace bp = boost::python;\nusing namespace simple::python;\n\nBOOST_PYTHON_MODULE(SIMPLE_PYTHON_MODULE_NAME)\n{\n bp::docstring_options module_docstring_options(true, true, false);\n\n bp::scope().attr(\"__version__\") = bp::str(SIMPLE_VERSION);\n bp::scope().attr(\"__raw_version__\") = bp::str(SIMPLE_VERSION);\n\n eigenpy::enableEigenPy();\n using Matrix6x3s = Eigen::Matrix;\n eigenpy::enableEigenPySpecific();\n\n // Enable warnings\n bp::import(\"warnings\");\n\n // Dependencies\n bp::import(\"hppfcl\");\n bp::import(\"pinocchio\");\n\n exposeContactFrame();\n exposeConstraintsProblem();\n exposeSimulator();\n}\n" }, { "path": "bindings/python/simple/__init__.py", "content": "#\n# Copyright (c) 2024 INRIA\n#\n\nimport pinocchio\nimport hppfcl\nfrom .simple_pywrap import *\nfrom .simple_pywrap import __version__, __raw_version__\n" }, { "path": "include/simple/bindings/python/core/constraints-problem.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_python_core_constraints_problem_hpp__\n#define __simple_python_core_constraints_problem_hpp__\n\n#include \"simple/core/constraints-problem.hpp\"\n#include \"simple/bindings/python/fwd.hpp\"\n\n#include \n#include \n\n#define SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(type, name, desc) \\\n add_property( \\\n #name, bp::make_function(+[](Self & self) -> type { return self.name(); }), \\\n bp::make_function(+[](Self &, const type &) -> void { PINOCCHIO_THROW(std::invalid_argument, \"Cannot manually set \" #name \".\") }), \\\n desc)\n\nnamespace simple\n{\n namespace python\n {\n namespace bp = boost::python;\n\n // template\n // struct StdVectorOfReferenceWrappersPythonVisitor : public bp::def_visitor>\n // {\n // typedef std::vector> Self;\n //\n // public:\n // template\n // void visit(PyClass & cl) const\n // {\n // cl.def(\n // \"__getitem__\", +[](const Self & self, std::size_t i) -> const T & { return self[i].get(); },\n // bp::return_value_policy())\n // .def(\"__len__\", +[](const Self & self) -> std::size_t { return self.size(); });\n // }\n //\n // static void expose(const std::string & name)\n // {\n // bp::class_(name.c_str(), \"Vector of reference wrappers.\", bp::no_init).def(StdVectorOfReferenceWrappersPythonVisitor());\n // }\n // };\n\n template\n struct ConstraintsProblemPythonVisitor : public bp::def_visitor>\n {\n typedef typename ConstraintsProblem::Scalar Scalar;\n typedef ConstraintsProblem Self;\n\n using ModelHandle = typename Self::ModelHandle;\n using DataHandle = typename Self::DataHandle;\n using VectorXs = typename Self::VectorXs;\n using GeometryModelHandle = typename Self::GeometryModelHandle;\n using GeometryDataHandle = typename Self::GeometryDataHandle;\n using PlacementVector = typename Self::PlacementVector;\n using ConstraintModel = typename Self::ConstraintModel;\n using ConstraintData = typename Self::ConstraintData;\n using BilateralPointConstraintModelVector = typename Self::BilateralPointConstraintModelVector;\n using WeldConstraintModelVector = typename Self::WeldConstraintModelVector;\n\n template\n void visit(PyClass & cl) const\n {\n cl\n // ----------------------------------\n // CONSTRUCTORS\n .def(bp::init(\n bp::args(\"self\", \"model\", \"data\", \"geom_model\", \"geom_data\"), \"Constructor\"))\n\n // ----------------------------------\n // ATTRIBUTES/METHODS\n\n // ----------------------------------\n // -- general\n .def_readonly(\n \"constraint_cholesky_decomposition\", &Self::constraint_cholesky_decomposition,\n \"Cholesky decomposition of the constraints problem. In particular, it contains the \"\n \"Cholesky decomposition of the Delassus' operator `G`.\")\n .def_readwrite(\n \"is_ncp\", &Self::is_ncp, \"Type of constraints problem. If set to true, the constraints problem is a NCP, else it is a CCP.\")\n\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(int, constraints_problem_size, \"Size of the full constraint problem vector.\")\n\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, g, \"Drift term `g` of the constraints problem.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, preconditioner, \"Preconditioner of the constraints problem.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, constraints_forces, \"Constraint forces of the current contact problem.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, previous_constraints_forces, \"Constraint forces of the previous contact problem.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, constraints_velocities, \"Constraint velocities divided by dt (v_c / dt).\")\n\n .def(\"update\", &Self::update, bp::args(\"self\"), \"Update constraints with current model, data, geom_model, geom_data.\")\n\n .def(\"clear\", &Self::clear, bp::args(\"self\"), \"Clear currrent contact quantities.\")\n\n // ----------------------------------\n // -- joint friction\n .def_readwrite(\"joint_friction_constraint_models\", &Self::joint_friction_constraint_model, \"Joint friction constraint model.\")\n .def_readonly(\"joint_friction_constraint_datas\", &Self::joint_friction_constraint_data, \"Joint friction constraint data.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n int, joint_friction_constraint_size, \"Size of the vector of dry friction force vector.\")\n .def(\n \"getNumberOfJointFrictionConstraints\", &Self::getNumberOfJointFrictionConstraints, bp::args(\"self\"),\n \"Returns the number of joint friction constraints.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, joint_friction_constraint_forces, \"Joint friction constraints' forces.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, previous_joint_friction_constraint_forces, \"Joint friction constraints' forces at the previous time step.\")\n\n // ----------------------------------\n // -- bilateral constraints\n .def_readwrite(\n \"bilateral_point_constraint_models\", &Self::bilateral_point_constraint_models, \"Vector of bilateral constraint models.\")\n .def_readonly(\n \"bilateral_point_constraint_datas\", &Self::bilateral_point_constraint_datas, \"Vector of bilateral constraint datas.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n int, bilateral_constraints_size, \"Size of the vector of bilateral constraint force vector.\")\n .def(\n \"getNumberOfBilateralConstraints\", &Self::getNumberOfBilateralConstraints, bp::args(\"self\"),\n \"Returns the number of bilateral constraints.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, bilateral_constraints_forces, \"Bilateral constraints' forces.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, previous_bilateral_constraints_forces, \"Bilateral constraints' forces at the previous time step.\")\n\n // ----------------------------------\n // -- weld constraints\n .def_readwrite(\"weld_constraint_models\", &Self::weld_constraint_models, \"Vector of weld constraint models.\")\n .def_readonly(\"weld_constraint_models\", &Self::weld_constraint_models, \"Vector of weld constraint datas.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n int, weld_constraints_size, \"Size of the vector of weld constraint force vector.\")\n .def(\"getNumberOfWeldConstraints\", &Self::getNumberOfWeldConstraints, bp::args(\"self\"), \"Returns the number of weld constraints.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, weld_constraints_forces, \"Weld constraints' forces.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, previous_weld_constraints_forces, \"Weld constraints' forces at the previous time step.\")\n\n // ----------------------------------\n // -- joint limits\n .def_readwrite(\"joint_limit_constraint_model\", &Self::joint_limit_constraint_model, \"Joint limit constraint model.\")\n .def_readonly(\"joint_limit_constraint_data\", &Self::joint_limit_constraint_data, \"Joint limit constraint data.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(int, joint_limit_constraint_size, \"Size of joint limit forces vector.\")\n .def(\n \"getNumberOfJointLimitConstraints\", &Self::getNumberOfJointLimitConstraints, bp::args(\"self\"),\n \"Returns the number of joint limit constraints.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, joint_limit_constraint_forces, \"Joint limit constraint's forces.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, previous_joint_limit_constraint_forces, \"Joint limit constraint's forces at the previous time step.\")\n\n // ----------------------------------\n // -- frictional point constraints\n .def_readwrite(\n \"frictional_point_constraint_models\", &Self::frictional_point_constraint_models,\n \"Vector of frictional point constraint models.\")\n .def_readonly(\n \"frictional_point_constraint_datas\", &Self::frictional_point_constraint_datas, \"Vector of frictional point constraint datas.\")\n .def(\n \"setMaxNumberOfContactsPerCollisionPair\", &Self::setMaxNumberOfContactsPerCollisionPair, bp::args(\"self\"),\n \"Set maximum number of contacts for each collision pair.\")\n .def(\n \"getMaxNumberOfContactsPerCollisionPair\", &Self::getMaxNumberOfContactsPerCollisionPair, bp::args(\"self\"),\n \"Get the maximum number of contacts for each collision pair.\")\n .def(\n \"getMaxNumberOfContacts\", &Self::getMaxNumberOfContacts, bp::args(\"self\"),\n \"Maximum number of contacts this `ConstraintsProblem` can handle.\")\n\n .def(\"getNumberOfContacts\", &Self::getNumberOfContacts, bp::args(\"self\"), \"Number of contacts.\")\n .def(\"getPreviousNumberOfContacts\", &Self::getPreviousNumberOfContacts, bp::args(\"self\"), \"Previous number of contacts.\")\n\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n int, frictional_point_constraints_size, \"Size of the vector of the frictional point constraints' forces.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, frictional_point_constraints_forces, \"Frictional point constraints' forces.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, previous_frictional_point_constraints_forces, \"Frictional point constraints' forces at the previous time step.\")\n\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n VectorXs, frictional_point_constraints_velocities, \"Frictional point constraints' velocities scaled by dt (v_c * dt).\")\n\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n PlacementVector, frictional_point_constraints_placements, \"Contact placements (oMc) of the current contact problem.\")\n .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(\n PlacementVector, previous_frictional_point_constraints_placements,\n \"Contact placements (oMc) at the previous time step (previous contact problem).\")\n\n .def_readonly(\"pairs_in_collision\", &Self::pairs_in_collision, \"Ids of collision pairs which are in collision.\")\n .def_readonly(\n \"contact_id_to_collision_pair\", &Self::contact_id_to_collision_pair,\n \"Vector that maps the id of the contact to the collision pair. Therefore \"\n \"`contact_id_to_collision_pair[i]` is the id of the collision pair corresponding to \"\n \"the i-th contact. Note: since collision pairs can have multiple contacts, the same id \"\n \"can be found multiple times inside `contact_id_to_collision_pair`.\")\n .def_readonly(\"contact_mappers\", &Self::contact_mappers, \"Vector of contact mappers of the current contact problem.\")\n\n .def_readonly(\n \"contact_modes\", &Self::contact_modes,\n \"Contact modes associated to frictional point constraints (breadking, sliding or sticking).\")\n .def(\n \"collectActiveSet\", &Self::collectActiveSet, (bp::arg(\"self\"), bp::arg(\"epsilon\") = 1e-6),\n \"Collect active set of of the solution of the contact problem.\");\n\n // StdVectorOfReferenceWrappersPythonVisitor::expose(\"StdRefVec_ConstraintModel\");\n // StdVectorOfReferenceWrappersPythonVisitor::expose(\"StdRefVec_ConstraintData\");\n\n cl.def_readonly(\"constraint_models\", &Self::constraint_models, \"Active constraint models.\")\n .def_readonly(\"constraint_datas\", &Self::constraint_datas, \"Active constraint datas.\");\n }\n\n static void expose()\n {\n ::pinocchio::python::StdVectorPythonVisitor::expose(\n \"StdVec_BilateralPointConstraintModel\");\n\n ::pinocchio::python::StdVectorPythonVisitor::expose(\n \"StdVec_WeldConstraintModel\");\n\n ::pinocchio::python::StdVectorPythonVisitor, true>::expose(\"StdVec_double\");\n\n ::pinocchio::python::StdVectorPythonVisitor, true>::expose(\n \"StdVec_ContactMapper\");\n\n ::pinocchio::python::StdVectorPythonVisitor, true>::expose(\n \"StdVec_ContactMode\");\n\n bp::class_(\"WrappedConstraintModel\", bp::no_init)\n .def(\n \"__getattr__\",\n +[](bp::object self, std::string const & name) {\n using ConstraintModel = typename ConstraintsProblem::ConstraintModel;\n const ConstraintModel & obj = bp::extract(self());\n return bp::getattr(bp::object(bp::ptr(&obj)), name.c_str());\n })\n .def(\n \"__call__\",\n +[](const typename ConstraintsProblem::WrappedConstraintModel & cmodel)\n -> const typename ConstraintsProblem::ConstraintModel & { return cmodel.get(); },\n bp::return_internal_reference<>());\n\n bp::class_(\"WrappedConstraintData\", bp::no_init)\n .def(\n \"__getattr__\",\n +[](bp::object self, std::string const & name) {\n using ConstraintData = typename ConstraintsProblem::ConstraintData;\n const ConstraintData & obj = bp::extract(self());\n return bp::getattr(bp::object(bp::ptr(&obj)), name.c_str());\n })\n .def(\n \"__call__\",\n +[](const typename ConstraintsProblem::WrappedConstraintData & cdata) -> const typename ConstraintsProblem::ConstraintData & {\n return cdata.get();\n },\n bp::return_internal_reference<>());\n\n ::pinocchio::python::StdVectorPythonVisitor::expose(\n \"StdVec_WrappedConstraintModel\");\n ::pinocchio::python::StdVectorPythonVisitor::expose(\n \"StdVec_WrappedConstraintData\");\n\n bp::class_(\"ConstraintsProblem\", \"Contact problem.\\n\", bp::no_init)\n .def(ConstraintsProblemPythonVisitor())\n .def(::pinocchio::python::CopyableVisitor());\n }\n };\n\n } // namespace python\n} // namespace simple\n\n#undef SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY\n\n#endif // ifndef__simple_python_core_constraints_problem_hpp__\n" }, { "path": "include/simple/bindings/python/core/simulator.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_python_algorithm_simulator_hpp__\n#define __simple_python_algorithm_simulator_hpp__\n\n#include \"simple/bindings/python/fwd.hpp\"\n#include \"simple/core/simulator.hpp\"\n\n#include \n#include \n\n// clang-format off\n#define SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(type, name, desc) \\\n add_property( \\\n #name, \\\n bp::make_function( \\\n +[](Self & self) -> type & { return self.name(); }, bp::return_internal_reference<>()), \\\n bp::make_function(+[](Self &, const type &) -> void { \\\n PINOCCHIO_THROW(std::invalid_argument, \"Cannot manually set \" #name \". Use constructor instead.\") \\\n }), \\\n desc)\n// clang-format on\n\nnamespace simple\n{\n namespace python\n {\n namespace bp = boost::python;\n\n template\n struct SimulatorPythonVisitor : public boost::python::def_visitor>\n {\n using Self = Simulator;\n using Scalar = typename Self::Scalar;\n\n using ModelHandle = typename Self::ModelHandle;\n using Model = typename Self::Model;\n using DataHandle = typename Self::DataHandle;\n using Data = typename Self::Data;\n using GeometryModelHandle = typename Self::GeometryModelHandle;\n using GeometryModel = typename Self::GeometryModel;\n using GeometryDataHandle = typename Self::GeometryDataHandle;\n using GeometryData = typename Self::GeometryData;\n using BilateralPointConstraintModelVector = typename Self::BilateralPointConstraintModelVector;\n using WeldConstraintModelVector = typename Self::WeldConstraintModelVector;\n using ConstraintsProblemHandle = typename Self::ConstraintsProblemHandle;\n using VectorXs = typename Self::VectorXs;\n using MatrixXs = typename Self::MatrixXs;\n using SpatialForce = typename Self::SpatialForce;\n using SpatialForceVector = typename Self::SpatialForceVector;\n using ConstraintSolverType = typename Self::ConstraintSolverType;\n using ConstraintSolverSettings = typename Self::ConstraintSolverSettings;\n using ADMMConstraintSolverSettings = typename Self::ADMMConstraintSolverSettings;\n using PGSConstraintSolverSettings = typename Self::PGSConstraintSolverSettings;\n\n template\n void visit(PyClass & cl) const\n {\n cl\n // -----------------------------\n // CONSTRUCTORS\n .def(bp::init(\n bp::args(\"self\", \"model\", \"data\", \"geom_model\", \"geom_data\"), \"Constructor\"))\n .def(bp::init(\n bp::args(\"self\", \"model\", \"data\", \"geom_model\", \"geom_data\", \"bilateral_point_constraint_models\"), \"Constructor\"))\n .def(bp::init(\n bp::args(\"self\", \"model\", \"data\", \"geom_model\", \"geom_data\", \"weld_constraint_models\"), \"Constructor\"))\n .def(bp::init<\n ModelHandle, DataHandle, GeometryModelHandle, GeometryDataHandle, BilateralPointConstraintModelVector,\n WeldConstraintModelVector>(\n bp::args(\"self\", \"model\", \"data\", \"geom_model\", \"geom_data\", \"bilateral_point_constraint_models\", \"weld_constraint_models\"),\n \"Constructor\"))\n .def(bp::init(bp::args(\"self\", \"model\", \"geom_model\"), \"Constructor\"))\n .def(bp::init(\n bp::args(\"self\", \"model\", \"geom_model\", \"bilateral_point_constraint_models\"), \"Constructor\"))\n .def(bp::init(\n bp::args(\"self\", \"model\", \"geom_model\", \"weld_constraint_models\"), \"Constructor\"))\n .def(bp::init(\n bp::args(\"self\", \"model\", \"geom_model\", \"bilateral_point_constraint_models\", \"weld_constraint_models\"), \"Constructor\"))\n\n // -----------------------------\n // ATTRIBUTES\n .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(Model, model, \"Pinocchio model.\")\n .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(Data, data, \"Pinocchio data.\")\n .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(GeometryModel, geom_model, \"Pinocchio geometry model.\")\n .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(GeometryData, geom_data, \"Pinocchio geometry data.\")\n .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(ConstraintsProblem, constraints_problem, \"Constraint problem.\")\n\n // -- state\n .def_readonly(\"q\", &Self::q, \"Joints configuration of the system.\")\n .def_readonly(\"v\", &Self::v, \"Joints velocity.\")\n .def_readonly(\"tau\", &Self::tau, \"External joints torques.\")\n .def_readonly(\"fext\", &Self::fext, \"External joints forces.\")\n .def_readonly(\"dt\", &Self::dt, \"Time step used to compute next state.\")\n\n // -- new state\n .def_readonly(\"qnew\", &Self::qnew, \"New joints configuration of the system.\")\n .def_readonly(\"vfree\", &Self::vfree, \"New joints velocity as if there were no constraints.\")\n .def_readonly(\"vnew\", &Self::vnew, \"New joints velocity.\")\n .def_readonly(\"anew\", &Self::anew, \"New joints acceleration.\")\n .def_readonly(\n \"ftotal\", &Self::ftotal,\n \"Vector of total forces (external + constraint forces) applied on joints, expressed in the frame of each joint.\")\n .def_readonly(\n \"tau_total\", &Self::tau_total, \"Vector of total torques (given as input to step + constraint torques) applied on joints.\")\n .def_readonly(\"tau_constraints\", &Self::tau_constraints, \"Vector of constraint torques.\")\n\n // -- constraint solvers\n .def_readonly(\n \"constraint_solver_type\", &Self::constraint_solver_type, \"Type of constraint solver used in the last call to `step`.\")\n .def_readwrite(\"warm_start_constraints_forces\", &Self::warm_start_constraints_forces, \"Warm start constraint forces boolean.\")\n .def_readwrite(\n \"admm_constraint_solver_settings\", &Self::admm_constraint_solver_settings, \"Settings of the ADMM constraint solver.\")\n .def_readwrite(\"admm_constraint_solver\", &Self::admm_constraint_solver, \"The ADMM constraints solver.\")\n .def_readwrite(\"pgs_constraint_solver_settings\", &Self::pgs_constraint_solver_settings, \"Settings of the PGS constraint solver.\")\n .def_readwrite(\"pgs_constraint_solver\", &Self::pgs_constraint_solver, \"The PGS constraints solver.\")\n\n // -- misc\n .def_readwrite(\"measure_timings\", &Self::measure_timings, \"Measure timings of the `step` method.\")\n\n // -----------------------------\n // METHODS\n .def(\n \"reset\", &Self::reset, bp::args(\"self\"),\n \"Resets the simulator to accept a new initial state, i.e. before looping on the `step` method.\")\n //\n .def(\n \"step\",\n +[](Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, Scalar dt, std::size_t nsteps = 1) {\n self.template step<::pinocchio::ADMMContactSolverTpl>(q, v, tau, dt);\n for (std::size_t i = 0; i < nsteps - 1; ++i)\n {\n self.template step<::pinocchio::ADMMContactSolverTpl>(self.qnew, self.vnew, tau, dt);\n }\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"tau\"), bp::arg(\"dt\"), bp::arg(\"nsteps\") = 1),\n \"Do one step of simulation with default constraint solver (ADMM).\")\n //\n .def(\n \"rollout\",\n +[](Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, Scalar dt, std::size_t nsteps = 1) {\n VectorXs q_ = q;\n VectorXs v_ = v;\n VectorXs tau_ = tau;\n Py_BEGIN_ALLOW_THREADS;\n self.template step<::pinocchio::ADMMContactSolverTpl>(q_, v_, tau_, dt);\n for (std::size_t i = 0; i < nsteps - 1; ++i)\n {\n self.template step<::pinocchio::ADMMContactSolverTpl>(self.qnew, self.vnew, tau, dt);\n }\n Py_END_ALLOW_THREADS;\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"tau\"), bp::arg(\"dt\"), bp::arg(\"nsteps\") = 1),\n \" Compute a trajectory by performing a rollout of a policy. This function releases \"\n \"Python GIL so it can be parallelized (input vectors are copied for safety). TODO: in \"\n \"the future, rollout should take a control policy as input rather than a constant \"\n \"torque.\")\n //\n .def(\n \"rollout\",\n +[](Self & self, const VectorXs & q, const VectorXs & v, const std::vector & taus, Scalar dt) {\n VectorXs q_ = q;\n VectorXs v_ = v;\n Py_BEGIN_ALLOW_THREADS;\n for (const auto & tau : taus)\n {\n self.template step<::pinocchio::ADMMContactSolverTpl>(q_, v_, tau, dt);\n q_ = self.qnew;\n v_ = self.vnew;\n }\n Py_END_ALLOW_THREADS;\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"taus\"), bp::arg(\"dt\")),\n \"Compute a trajectory by performing a rollout of a policy with a list of taus. This function releases \"\n \"Python GIL so it can be parallelized (input vectors are copied for safety).\")\n //\n .def(\n \"rollout\",\n +[](Self & self, const VectorXs & q, const VectorXs & v, const MatrixXs & taus, Scalar dt) {\n VectorXs q_ = q;\n VectorXs v_ = v;\n Py_BEGIN_ALLOW_THREADS;\n for (int i = 0; i < taus.rows(); ++i)\n {\n VectorXs tau = taus.row(i);\n self.template step<::pinocchio::ADMMContactSolverTpl>(q_, v_, tau, dt);\n q_ = self.qnew;\n v_ = self.vnew;\n }\n Py_END_ALLOW_THREADS;\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"taus\"), bp::arg(\"dt\")),\n \"Compute a trajectory by performing a rollout of a policy with a matrix of taus. This function releases \"\n \"Python GIL so it can be parallelized (input vectors are copied for safety).\")\n //\n .def(\n \"step\",\n +[](\n Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, const SpatialForceVector & fext, Scalar dt,\n std::size_t nsteps = 1) {\n self.template step<::pinocchio::ADMMContactSolverTpl>(q, v, tau, fext, dt);\n for (std::size_t i = 0; i < nsteps - 1; ++i)\n {\n self.template step<::pinocchio::ADMMContactSolverTpl>(self.qnew, self.vnew, tau, fext, dt);\n }\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"tau\"), bp::arg(\"fext\"), bp::arg(\"dt\"), bp::arg(\"nsteps\") = 1),\n \"Do one step of simulation with default constraint solver (ADMM).\")\n //\n .def(\n \"stepPGS\",\n +[](Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, Scalar dt, std::size_t nsteps = 1) {\n self.template step<::pinocchio::PGSContactSolverTpl>(q, v, tau, dt);\n for (std::size_t i = 0; i < nsteps - 1; ++i)\n {\n self.template step<::pinocchio::PGSContactSolverTpl>(self.qnew, self.vnew, tau, dt);\n }\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"tau\"), bp::arg(\"dt\"), bp::arg(\"nsteps\") = 1),\n \"Do one step of simulation with PGS constraint solver.\")\n //\n .def(\n \"stepPGS\",\n +[](\n Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, const SpatialForceVector & fext, Scalar dt,\n std::size_t nsteps = 1) {\n self.template step<::pinocchio::PGSContactSolverTpl>(q, v, tau, fext, dt);\n for (std::size_t i = 0; i < nsteps - 1; ++i)\n {\n self.template step<::pinocchio::PGSContactSolverTpl>(self.qnew, self.vnew, tau, fext, dt);\n }\n },\n (bp::arg(\"self\"), bp::arg(\"q\"), bp::arg(\"v\"), bp::arg(\"tau\"), bp::arg(\"fext\"), bp::arg(\"dt\"), bp::arg(\"nsteps\") = 1),\n \"Do one step of simulation with PGS constraint solver.\")\n\n .def(\"getStepCPUTimes\", &Self::getStepCPUTimes, bp::args(\"self\"), \"Get timings of the last call to the `step` method.\")\n //\n .def(\n \"getConstraintSolverCPUTimes\", &Self::getConstraintSolverCPUTimes, bp::args(\"self\"),\n \"Get timings of the call to the constraint solver in the last call to the `step` method. \"\n \"These timings can be 0 if the system has no constraints.\")\n //\n .def(\n \"getCollisionDetectionCPUTimes\", &Self::getCollisionDetectionCPUTimes, bp::args(\"self\"),\n \"Get timings of the collision detection stage.\");\n\n // Register handles\n {\n // Check registration\n {\n const bp::type_info info = bp::type_id();\n const bp::converter::registration * reg = bp::converter::registry::query(info);\n if (!reg)\n {\n bp::register_ptr_to_python();\n }\n }\n {\n const bp::type_info info = bp::type_id();\n const bp::converter::registration * reg = bp::converter::registry::query(info);\n if (!reg)\n {\n bp::register_ptr_to_python();\n }\n }\n {\n const bp::type_info info = bp::type_id();\n const bp::converter::registration * reg = bp::converter::registry::query(info);\n if (!reg)\n {\n bp::register_ptr_to_python();\n }\n }\n {\n const bp::type_info info = bp::type_id();\n const bp::converter::registration * reg = bp::converter::registry::query(info);\n if (!reg)\n {\n bp::register_ptr_to_python();\n }\n }\n }\n }\n\n static void expose()\n {\n bp::class_(\"Simulator\", \"Instance of Simulator.\", bp::no_init)\n .def(SimulatorPythonVisitor())\n .def(::pinocchio::python::CopyableVisitor());\n\n {\n bp::enum_(\"ConstraintSolverType\")\n .value(\"PGS\", ConstraintSolverType::PGS)\n .value(\"ADMM\", ConstraintSolverType::ADMM)\n .value(\"NONE\", ConstraintSolverType::NONE);\n }\n\n {\n bp::class_(\n \"ConstraintSolverSettings\", \"Settings common to all constraint solvers inside `Simulator`.\", bp::no_init)\n .def_readwrite(\"max_iter\", &ConstraintSolverSettings::max_iter, \"Max number of iteration of the constraint solver.\")\n .def_readwrite(\n \"absolute_precision\", &ConstraintSolverSettings::absolute_precision,\n \"Absolute convergence precision of the constraint solver.\")\n .def_readwrite(\n \"relative_precision\", &ConstraintSolverSettings::relative_precision,\n \"Relative convergence precision of the constraint solver.\")\n .def_readwrite(\"stat_record\", &ConstraintSolverSettings::stat_record, \"Record metrics of the constraint solver.\");\n }\n\n {\n bp::class_>(\n \"ADMMConstraintSolverSettings\", \"Settings for the ADMM constraint solver inside `Simulator`.\", bp::no_init)\n .def_readwrite(\"mu\", &ADMMConstraintSolverSettings::mu, \"Proximal parameter of ADMM.\")\n .def_readwrite(\n \"tau\", &ADMMConstraintSolverSettings::tau, \"ADMM augmented lagragian penalty is tau * rho (rho is scaled during iterations).\")\n .def_readwrite(\"rho\", &ADMMConstraintSolverSettings::rho, \"Initial value of rho when using the linear update rule.\")\n .def_readwrite(\n \"rho_power\", &ADMMConstraintSolverSettings::rho_power, \"Initial value of rho_power when using the spectral update rule.\")\n .def_readwrite(\"rho_power_factor\", &ADMMConstraintSolverSettings::rho_power_factor, \"Update factor on rho_power.\")\n .def_readwrite(\n \"linear_update_rule_factor\", &ADMMConstraintSolverSettings::linear_update_rule_factor,\n \"Update factor on rho when using linear update rule.\")\n .def_readwrite(\n \"ratio_primal_dual\", &ADMMConstraintSolverSettings::ratio_primal_dual, \"Ratio above/below which to trigger the rho update.\")\n .def_readwrite(\n \"lanczos_size\", &ADMMConstraintSolverSettings::lanczos_size,\n \"Size of Lanczos decomposition. Higher yields more accurate delassus eigenvalues estimates.\")\n .def_readwrite(\n \"admm_update_rule\", &ADMMConstraintSolverSettings::admm_update_rule,\n \"Update rule for the ADMM constraint solver (linear or spectral)\");\n }\n\n {\n bp::class_>(\n \"PGSConstraintSolverSettings\", \"Settings for the PGS constraint solver inside `Simulator`.\", bp::no_init)\n .def_readwrite(\"over_relax\", &PGSConstraintSolverSettings::over_relax, \"Optional over relaxation value, default to 1.\");\n }\n }\n };\n\n } // namespace python\n} // namespace simple\n\n#undef SIMPLE_SIMULATOR_ADD_READONLY_HANDLE\n\n#endif // ifndef __simple_python_algorithm_simulator_hpp__\n" }, { "path": "include/simple/bindings/python/fwd.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_python_fwd_hpp__\n#define __simple_python_fwd_hpp__\n\n#include \n\nnamespace simple\n{\n namespace python\n {\n void exposeContactFrame();\n void exposeConstraintsProblem();\n void exposeSimulator();\n } // namespace python\n} // namespace simple\n\n#endif // #ifndef __simple_python_fwd_hpp__\n" }, { "path": "include/simple/core/constraints-problem.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_constraints_problem_hpp__\n#define __simple_core_constraints_problem_hpp__\n\n#include \"simple/core/fwd.hpp\"\n#include \"simple/core/contact-frame.hpp\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace simple\n{\n\n template class JointCollectionTpl>\n struct traits>\n {\n using Scalar = _Scalar;\n };\n\n template class JointCollectionTpl>\n struct ConstraintsProblemTpl // : ::pinocchio::NumericalBase>\n {\n // TODO: template by allocator\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n /////////////////////////////////////////////////\n /// TYPEDEFS\n /////////////////////////////////////////////////\n using Scalar = _Scalar;\n enum\n {\n Options = _Options\n };\n\n using GeomIndex = pinocchio::GeomIndex;\n using JointIndex = pinocchio::JointIndex;\n\n using VectorXs = Eigen::Matrix;\n using MatrixXs = Eigen::Matrix;\n using Vector3s = Eigen::Matrix;\n using Vector6s = Eigen::Matrix;\n using Matrix3s = Eigen::Matrix;\n using MapVectorXs = Eigen::Map;\n using MapVector3s = Eigen::Map;\n using MapVector6s = Eigen::Map;\n using ContactIndex = std::size_t;\n\n using Model = ::pinocchio::ModelTpl;\n using ModelHandle = std::shared_ptr;\n using Data = typename Model::Data;\n using DataHandle = std::shared_ptr;\n using GeometryModelHandle = std::shared_ptr<::pinocchio::GeometryModel>;\n using GeometryDataHandle = std::shared_ptr<::pinocchio::GeometryData>;\n\n using ConstraintModel = ::pinocchio::ConstraintModelTpl;\n using ConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(ConstraintModel);\n using WrappedConstraintModel = std::reference_wrapper;\n using WrappedConstraintModelVector = std::vector;\n\n using ConstraintData = ::pinocchio::ConstraintDataTpl;\n using ConstraintDataVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(ConstraintData);\n using WrappedConstraintData = std::reference_wrapper;\n using WrappedConstraintDataVector = std::vector;\n\n using FrictionalJointConstraintModel = ::pinocchio::FrictionalJointConstraintModelTpl;\n using FrictionalJointConstraintData = ::pinocchio::FrictionalJointConstraintDataTpl;\n\n using BilateralPointConstraintModel = ::pinocchio::BilateralPointConstraintModelTpl;\n using BilateralPointConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(BilateralPointConstraintModel);\n using BilateralPointConstraintData = ::pinocchio::BilateralPointConstraintDataTpl;\n\n using WeldConstraintModel = ::pinocchio::WeldConstraintModelTpl;\n using WeldConstraintData = ::pinocchio::WeldConstraintDataTpl;\n using WeldConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(WeldConstraintModel);\n\n using JointLimitConstraintModel = ::pinocchio::JointLimitConstraintModelTpl;\n using JointLimitConstraintData = ::pinocchio::JointLimitConstraintDataTpl;\n using BoxSet = ::pinocchio::BoxSetTpl;\n\n using FrictionalPointConstraintModel = ::pinocchio::FrictionalPointConstraintModelTpl;\n using FrictionalPointConstraintData = ::pinocchio::FrictionalPointConstraintDataTpl;\n using CoulombFrictionCone = ::pinocchio::CoulombFrictionConeTpl;\n using CoulombFrictionConeVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(CoulombFrictionCone);\n using ConstraintCholeskyDecomposition = ::pinocchio::ContactCholeskyDecompositionTpl;\n using ContactPointVector = PINOCCHIO_ALIGNED_STD_VECTOR(Vector3s);\n using ContactNormalVector = PINOCCHIO_ALIGNED_STD_VECTOR(Vector3s);\n using SE3 = ::pinocchio::SE3Tpl;\n using PlacementVector = PINOCCHIO_ALIGNED_STD_VECTOR(SE3);\n // operator to compute contact frames\n using PlacementFromNormalAndPosition = PlacementFromNormalAndPositionTpl;\n\n /////////////////////////////////////////////////\n /// ATTRIBUTES\n /////////////////////////////////////////////////\n\n /// ----------------------------------\n /// General attributes\n\n public:\n /// \\brief Cholesky decomposition of the constraints problem.\n ConstraintCholeskyDecomposition constraint_cholesky_decomposition;\n\n /// \\brief Vector of constraint models currently active in the constraints problem.\n WrappedConstraintModelVector constraint_models;\n\n /// \\brief Vector of constraint models' data.\n WrappedConstraintDataVector constraint_datas;\n\n /// \\brief Wether the constraints problem should be treated as a NCP or a CCP.\n /// NCP = Non-linear Complementarity Problem\n /// CCP = Convex Complementarity Problem\n bool is_ncp{true};\n\n protected:\n /// \\brief Handle to the model of the system.\n ModelHandle m_model;\n\n /// \\brief Handle to the model's data of the system.\n DataHandle m_data;\n\n /// \\brief Handle to the geometry model of the system.\n GeometryModelHandle m_geom_model;\n\n /// \\brief Handle to the geometry model's data of the system.\n GeometryDataHandle m_geom_data;\n\n /// \\brief Drift term of the constraints problem.\n VectorXs m_g;\n\n /// \\brief Preconditionner of the constraints problem.\n VectorXs m_preconditioner;\n\n /// \\brief Velocity of constraints.\n VectorXs m_constraints_velocities;\n\n /// \\brief Warm start for the constraint solver\n VectorXs m_constraints_velocities_warmstarts;\n\n /// \\brief Holders for constraint forces (current and previous).\n std::array m_constraints_forces_holder;\n\n /// \\brief Current constraints problem holder id.\n std::size_t m_current_constraints_pb_id;\n\n /// \\brief Vector of time scaling factors to convert acceleration units to the units of each constraint.\n VectorXs m_time_scaling_acc_to_constraints;\n\n /// ----------------------------------\n /// Joints dry frictions constraint\n\n public:\n /// \\brief Frictional joint constraint model.\n ConstraintModel joint_friction_constraint_model;\n\n /// \\brief Frictional joint constraint data.\n ConstraintData joint_friction_constraint_data;\n\n /// ----------------------------------\n /// Bilateral constraints\n\n /// \\brief Holder for bilateral point constraint models.\n ConstraintModelVector bilateral_point_constraint_models;\n\n /// \\brief Holder for bilateral point constraint datas.\n ConstraintDataVector bilateral_point_constraint_datas;\n\n /// ----------------------------------\n /// Weld constraints\n\n /// \\brief Holder for weld point constraint models.\n ConstraintModelVector weld_constraint_models;\n\n /// \\brief Holder for weld point constraint datas.\n ConstraintDataVector weld_constraint_datas;\n\n /// ----------------------------------\n /// Joint limits constraint\n\n /// \\brief Joint Limit constraint model.\n ConstraintModel joint_limit_constraint_model;\n\n /// \\brief Joint Limit constraint data.\n ConstraintData joint_limit_constraint_data;\n\n /// ----------------------------------\n /// Frictional point constraints\n\n /// \\brief Holder for frictional point constraint models.\n ConstraintModelVector frictional_point_constraint_models;\n\n /// \\brief Holder for frictional point constraint datas.\n ConstraintDataVector frictional_point_constraint_datas;\n\n /// \\brief Vector of collision pair ids that are in collision.\n std::vector pairs_in_collision;\n\n /// \\brief The collision pairs that were in contact at the previous time step (before update function is called).\n std::vector previous_colliding_collision_pairs;\n\n /// \\brief Vector that maps the id of the contact to the collision pair.\n /// Therefore `contact_id_to_collision_pair[i]` is the id of the collision pair\n /// corresponding to the i-th contact.\n /// Note: since collision pairs can have multiple contacts, the same id can be found\n /// multiple times inside `contact_id_to_collision_pair`.\n std::vector contact_id_to_collision_pair;\n\n /// \\brief Since each collision pair can have multiple contact points, this struct maps a single\n /// collision pair to contact points information: contact position, contact normal, contact\n /// placements, constraint models/datas, friction cones, elasticities and penetration depths.\n struct ContactMapper\n {\n ContactMapper(std::size_t begin, std::size_t count)\n : begin(begin)\n , count(count)\n {\n }\n /// \\brief The id of the first contact point for the considered collision pair.\n std::size_t begin;\n\n /// \\brief Number of contact points for the considered collision pair.\n std::size_t count;\n };\n\n /// \\brief Vector of contact mappers.\n /// The i-th element of this vector is the contact mapper for the i-th colliding collision pair.\n std::vector contact_mappers;\n\n /// \\brief A contact can either be:\n /// - breaking (no contact force)\n /// - sticking (contact force inside the friction cone)\n /// - sliding (contact force saturating the friction cone)\n // TODO this should be moved to constraints\n enum struct ContactMode\n {\n BREAKING,\n STICKING,\n SLIDING,\n };\n\n /// \\brief vector of contact modes\n std::vector contact_modes;\n\n /// \\brief indexes of breaking contacts\n std::vector breaking_contacts;\n\n /// \\brief indexes of sticking contacts\n std::vector sticking_contacts;\n\n /// \\brief indexes of sliding contacts\n std::vector sliding_contacts;\n\n protected:\n /// \\brief Maximum number of contact points per collision pair.\n ContactIndex m_max_num_contact_per_collision_pair;\n\n /// \\brief Number of contact points after `update` is called.\n ContactIndex m_num_contacts;\n\n /// \\brief Number of contact points in previous time step.\n ContactIndex m_previous_num_contacts;\n\n /// \\brief Holders for contact placements (current and previous).\n std::array m_frictional_point_constraints_placements_holder;\n\n /// \\brief Size of vector of previous joint limit constraint forces.\n int m_previous_joint_limits_constraint_forces_size;\n\n /// \\brief Active set of previous joint limit constraint model.\n std::vector m_previous_joint_limit_active_set;\n\n /////////////////////////////////////////////////\n /// METHODS\n /////////////////////////////////////////////////\n public:\n /// ----------------------------------\n /// Constructors\n\n /// \\brief Default constructor.\n ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_point_constraint_models,\n const WeldConstraintModelVector & weld_constraint_models);\n\n /// \\brief Default constructor.\n ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_point_constraint_models);\n\n /// \\brief Default constructor.\n ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle,\n const WeldConstraintModelVector & weld_constraint_models);\n\n /// \\brief Default constructor.\n ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle);\n\n /// ----------------------------------\n /// General methods\n\n /// \\brief Returns a const reference to the model\n const Model & model() const\n {\n assert(this->m_model != nullptr);\n return pinocchio::helper::get_ref(this->m_model);\n }\n\n /// \\brief Returns a reference to the model\n Model & model()\n {\n assert(this->m_model != nullptr);\n return pinocchio::helper::get_ref(this->m_model);\n }\n\n /// \\brief Returns a const reference to the data\n const Data & data() const\n {\n assert(this->m_data != nullptr);\n return pinocchio::helper::get_ref(this->m_data);\n }\n\n /// \\brief Returns a reference to the data\n Data & data()\n {\n assert(this->m_data != nullptr);\n return pinocchio::helper::get_ref(this->m_data);\n }\n\n /// \\brief Returns a const reference to the geometry model\n const pinocchio::GeometryModel & geom_model() const\n {\n assert(this->m_geom_model != nullptr);\n return pinocchio::helper::get_ref(this->m_geom_model);\n }\n\n /// \\brief Returns a reference to the geometry model\n pinocchio::GeometryModel & geom_model()\n {\n assert(this->m_geom_model != nullptr);\n return pinocchio::helper::get_ref(this->m_geom_model);\n }\n\n /// \\brief Returns a const reference to the geometry data\n const pinocchio::GeometryData & geom_data() const\n {\n assert(this->m_geom_data != nullptr);\n return pinocchio::helper::get_ref(this->m_geom_data);\n }\n\n /// \\brief Returns a reference to the geometry data\n pinocchio::GeometryData & geom_data()\n {\n assert(this->m_geom_data != nullptr);\n return pinocchio::helper::get_ref(this->m_geom_data);\n }\n\n /// \\brief Allocates memory for the constraints problem quantities.\n /// Notes:\n /// - This method uses the the geometry model's active collision pairs to allocate memory.\n /// - because we always resize the constraints problem quantities, there won't be any error if\n /// this method is not called.\n /// This method is meant to optimize memory allocation for advanced users.\n void allocate();\n\n /// \\brief Empties constraints problem quantities.\n void clear();\n\n /// \\brief After `model`, `data`, `geom_model` and `geom_data` have been updated, this function\n /// updates `constraints`. \\param compute_warm_start whether or not to compute a warm-start for\n /// the constraints forces, based on the previous solution of the constraints problem.\n void update(const bool compute_warm_start);\n\n /// \\brief Build the constraints problem quantities: `G`, `g`.\n /// Also builds the quantities necessary to warm-start the constraint solver.\n /// Meant to be called after `update`.\n template\n void build(\n const Eigen::MatrixBase & vfree,\n const Eigen::MatrixBase & v,\n const Eigen::MatrixBase & v_warmstart,\n Scalar dt);\n\n /// \\brief Checks consistency of the constraints problem w.r.t to its handles.\n bool check() const;\n\n /// \\brief Size of the constraints problem.\n int constraints_problem_size() const\n {\n return this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size() //\n + this->frictional_point_constraints_size();\n }\n\n /// \\brief Size of the previous constraints problem.\n int previous_constraints_problem_size() const\n {\n return this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->previous_joint_limit_constraint_size() //\n + this->previous_frictional_point_constraints_size();\n }\n\n /// \\brief Getter for the `g` term (i.e. the free velocity of constraints + corrective terms).\n Eigen::VectorBlock g()\n {\n return this->m_g.head(this->constraints_problem_size());\n }\n\n /// \\brief const getter for the `g` term (i.e. the free velocity of constraint + corrective terms).\n Eigen::VectorBlock g() const\n {\n return this->m_g.head(this->constraints_problem_size());\n }\n\n /// \\brief Getter for the preconditioner\n Eigen::VectorBlock preconditioner()\n {\n return this->m_preconditioner.head(this->constraints_problem_size());\n }\n\n /// \\brief Const getter for the preconditioner\n Eigen::VectorBlock preconditioner() const\n {\n return this->m_preconditioner.head(this->constraints_problem_size());\n }\n\n /// \\brief Getter for the time scaling factors to convert acceleration units to the units of each constraint.\n Eigen::VectorBlock time_scaling_acc_to_constraints()\n {\n return this->m_time_scaling_acc_to_constraints.head(this->constraints_problem_size());\n }\n\n /// \\brief Const getter for the time scaling factors to convert acceleration units to the units of each constraint.\n Eigen::VectorBlock time_scaling_acc_to_constraints() const\n {\n return this->m_time_scaling_acc_to_constraints.head(this->constraints_problem_size());\n }\n\n /// \\brief Getter for constraints' forces.\n Eigen::VectorBlock constraints_forces()\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->constraints_problem_size());\n }\n\n /// \\brief Const getter for constraints' forces.\n Eigen::VectorBlock constraints_forces() const\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->constraints_problem_size());\n }\n\n /// \\brief Getter for constraints' forces at the previous time step.\n Eigen::VectorBlock previous_constraints_forces()\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->previous_constraints_problem_size());\n }\n\n /// \\brief Const getter for constraints' forces at the previous time step.\n Eigen::VectorBlock previous_constraints_forces() const\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->previous_constraints_problem_size());\n }\n\n /// \\brief Getter for constraints' velocities.\n /// note: this should be mutltiplied by dt to retrieve the actual constraints' velocities.\n Eigen::VectorBlock constraints_velocities()\n {\n return this->m_constraints_velocities.head(this->constraints_problem_size());\n }\n\n /// \\brief Const getter for constraints' velocities.\n /// note: this should be mutltiplied by dt to retrieve the actual constraints' velocities.\n Eigen::VectorBlock constraints_velocities() const\n {\n return this->m_constraints_velocities.head(this->constraints_problem_size());\n }\n\n /// \\brief Getter for the the warm-start which uses the constraints' velocities and constraint-inverse dynamics.\n Eigen::VectorBlock constraints_velocities_warmstarts()\n {\n return this->m_constraints_velocities_warmstarts.head(this->constraints_problem_size());\n }\n\n /// \\brief Const getter for the the warm-start which uses the constraints' velocities and constraint-inverse dynamics.\n Eigen::VectorBlock constraints_velocities_warmstarts() const\n {\n return this->m_constraints_velocities_warmstarts.head(this->constraints_problem_size());\n }\n\n protected:\n /// \\brief Updates m_current_constraints_pb_id.\n void updateConstraintsHolders();\n\n /// \\brief Checks consistency of the constraints' model/data vectors w.r.t geom_model and geom_data.\n bool checkConstraintsConsistencyWithGeometryModel() const;\n\n /// \\brief Compute constraint drift g = Jc * vfree + baumgarte.\n /// For each constraint, the drift is expressed in the constraint formulation level\n /// of the constraint (position, velocity or acceleration)\n template\n void computeConstraintsDrift(\n const Eigen::MatrixBase & vfree, const Eigen::MatrixBase & v, const Scalar dt);\n\n public:\n /// ----------------------------------\n /// Joints dry frictions constraints\n\n /// \\brief Size of the joints dry friction force vector\n int joint_friction_constraint_size() const\n {\n return this->joint_friction_constraint_model.size();\n }\n\n /// \\brief Return the number of joint friction constraints.\n std::size_t getNumberOfJointFrictionConstraints() const\n {\n if (this->joint_friction_constraint_model.size() > 0)\n {\n return 1;\n }\n return 0;\n }\n\n /// \\brief Getter for friction forces on the joints.\n Eigen::VectorBlock joint_friction_constraint_forces()\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size());\n }\n\n /// \\brief Const getter for friction forces on the joints.\n Eigen::VectorBlock joint_friction_constraint_forces() const\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size());\n }\n\n /// \\brief Getter for previous friction forces on the joints.\n Eigen::VectorBlock previous_joint_friction_constraint_forces()\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size());\n }\n\n /// \\brief Const getter for previous friction forces on the joints.\n Eigen::VectorBlock previous_joint_friction_constraint_forces() const\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size());\n }\n\n /// ----------------------------------\n /// Bilateral constraints\n\n /// \\brief Size of the bilateral constraints.\n int bilateral_constraints_size() const\n {\n return (int)(3 * this->bilateral_point_constraint_models.size());\n }\n\n /// \\brief Return the number of bilateral constraints.\n std::size_t getNumberOfBilateralConstraints() const\n {\n return this->bilateral_point_constraint_models.size();\n }\n\n /// \\brief Getter for bilateral constraints' forces.\n Eigen::VectorBlock bilateral_constraints_forces()\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size(), this->bilateral_constraints_size());\n }\n\n /// \\brief Const getter for bilateral constraints' forces.\n Eigen::VectorBlock bilateral_constraints_forces() const\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size(), this->bilateral_constraints_size());\n }\n\n /// \\brief Getter for bilateral constraints' forces.\n Eigen::VectorBlock previous_bilateral_constraints_forces()\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size(), this->bilateral_constraints_size());\n }\n\n /// \\brief Const getter for bilateral constraints' forces.\n Eigen::VectorBlock previous_bilateral_constraints_forces() const\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size(), this->bilateral_constraints_size());\n }\n\n /// ----------------------------------\n /// Weld constraints\n\n /// \\brief Size of the weld constraints.\n int weld_constraints_size() const\n {\n return (int)(6 * this->weld_constraint_models.size());\n }\n\n /// \\brief Return the number of weld constraints.\n std::size_t getNumberOfWeldConstraints() const\n {\n return this->weld_constraint_models.size();\n }\n\n /// \\brief Getter for weld constraints' forces.\n Eigen::VectorBlock weld_constraints_forces()\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size());\n }\n\n /// \\brief Const getter for weld constraints' forces.\n Eigen::VectorBlock weld_constraints_forces() const\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size());\n }\n\n /// \\brief Getter for previous weld constraints' forces.\n Eigen::VectorBlock previous_weld_constraints_forces()\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size());\n }\n\n /// \\brief Const getter for previous weld constraints' forces.\n Eigen::VectorBlock previous_weld_constraints_forces() const\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size());\n }\n\n /// ----------------------------------\n /// Joint limits constraint\n\n /// \\brief Maximum size of the joints limit force vector.\n int joint_limit_constraint_max_size() const\n {\n return this->joint_limit_constraint_model.size();\n }\n\n /// \\brief Size of the joints limit force vector.\n /// This corresponds to the size of the currently active joint limit constraint (i.e. the distance between the\n /// joint's position and the joint's limits is below a threshold).\n /// This requires the `update` function to be called before (as it itself calls `calc` on the joint limit constraint models/datas).\n int joint_limit_constraint_size() const\n {\n return this->joint_limit_constraint_model.activeSize();\n }\n\n /// \\brief Previous size of the joints limit force vector.\n int previous_joint_limit_constraint_size() const\n {\n return int(this->m_previous_joint_limits_constraint_forces_size);\n }\n\n /// \\brief Return the number of joint limit constraint.\n std::size_t getNumberOfJointLimitConstraints() const\n {\n if (this->joint_limit_constraint_model.activeSize() > 0)\n {\n return 1;\n }\n return 0;\n }\n\n /// \\brief Getter for forces from the limits on the joints.\n Eigen::VectorBlock joint_limit_constraint_forces()\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(),\n this->joint_limit_constraint_size());\n }\n\n /// \\brief Const getter for forces from the limits on the joints.\n Eigen::VectorBlock joint_limit_constraint_forces() const\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(),\n this->joint_limit_constraint_size());\n }\n\n /// \\brief Getter for forces from the previous limits on the joints.\n Eigen::VectorBlock previous_joint_limit_constraint_forces()\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(),\n this->previous_joint_limit_constraint_size());\n }\n\n /// \\brief Getter for forces from the previous limits on the joints.\n Eigen::VectorBlock previous_joint_limit_constraint_forces() const\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(),\n this->previous_joint_limit_constraint_size());\n }\n\n /// ----------------------------------\n /// Frictional point constraints\n\n /// \\brief Size of vector of frictional point constraints' forces.\n /// For example, if only 3D contacts are considered, this should be 3 * nc, where nc is the\n /// number of contacts obtained after calling the `update` method.\n int frictional_point_constraints_size() const\n {\n return (int)(3 * this->m_num_contacts);\n }\n\n /// \\brief Size of the previous contact problem.\n int previous_frictional_point_constraints_size() const\n {\n return (int)(3 * this->m_previous_num_contacts);\n }\n\n /// \\brief Sets the maximum number of contact points per collision pair.\n void setMaxNumberOfContactsPerCollisionPair(ContactIndex num)\n {\n if (num <= 0)\n {\n PINOCCHIO_THROW_PRETTY(std::logic_error, \"Cannot set max number of contacts per collision pair to a value <= 0.\");\n }\n this->m_max_num_contact_per_collision_pair = num;\n this->allocate();\n }\n\n /// \\brief Sets the maximum number of contact points per collision pair.\n ContactIndex getMaxNumberOfContactsPerCollisionPair() const\n {\n return this->m_max_num_contact_per_collision_pair;\n }\n\n /// \\brief Returns the maximum number of contact points this `ConstraintsProblem` can handle.\n /// This number determines the allocated memory of the `ConstraintsProblem`.\n /// To update the maximum number of contact points, please update the geom_model and call the\n /// `allocate` method to compute the updated maximum number of contact points.\n ContactIndex getMaxNumberOfContacts() const\n {\n return this->frictional_point_constraint_models.size();\n }\n\n /// \\brief Returns the number of contact points after `update` is called.\n ContactIndex getNumberOfContacts() const\n {\n return this->m_num_contacts;\n }\n\n /// \\brief Returns the previous number of contact points after `update` is called.\n ContactIndex getPreviousNumberOfContacts() const\n {\n return this->m_previous_num_contacts;\n }\n\n /// \\brief Getter for frictional point constraints' forces.\n Eigen::VectorBlock frictional_point_constraints_forces()\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Const getter for frictional point constraints' forces.\n Eigen::VectorBlock frictional_point_constraints_forces() const\n {\n return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Getter for frictional point constraints' forces at the previous time step.\n Eigen::VectorBlock previous_frictional_point_constraints_forces()\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->previous_joint_limit_constraint_size(),\n this->previous_frictional_point_constraints_size());\n }\n\n /// \\brief Const getter for frictional point constraints' forces at the previous time step.\n Eigen::VectorBlock previous_frictional_point_constraints_forces() const\n {\n return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->previous_joint_limit_constraint_size(),\n this->previous_frictional_point_constraints_size());\n }\n\n /// \\brief Getter for frictional point constraints' velocities.\n /// note: this should be mutltiplied by dt to retrieve the actual velocities.\n Eigen::VectorBlock frictional_point_constraints_velocities()\n {\n return this->m_constraints_velocities.segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Const getter for frictional point constraints' velocities.\n /// note: this should be mutltiplied by dt to retrieve the actual velocities.\n Eigen::VectorBlock frictional_point_constraints_velocities() const\n {\n return this->m_constraints_velocities.segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Getter for the the warm-start which uses the frictional point constraints' velocities and contact-inverse dynamics.\n Eigen::VectorBlock frictional_point_constraints_warmstart()\n {\n return this->m_constraints_velocities_warmstarts.segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Const getter for the the warm-start which uses the frictional point constraints' velocities and contact-inverse dynamics.\n Eigen::VectorBlock frictional_point_constraints_warmstart() const\n {\n return this->m_constraints_velocities_warmstarts.segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Getter for the time scaling factors to convert acceleration units to the units of each constraint.\n Eigen::VectorBlock contact_time_scaling_acc_to_constraints()\n {\n return this->m_time_scaling_acc_to_constraints.segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Const getter for the time scaling factors to convert acceleration units to the units of each constraint.\n Eigen::VectorBlock contact_time_scaling_acc_to_constraints() const\n {\n return this->m_time_scaling_acc_to_constraints.segment(\n this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_size(),\n this->frictional_point_constraints_size());\n }\n\n /// \\brief Getter for contact placements.\n PlacementVector & frictional_point_constraints_placements()\n {\n return this->m_frictional_point_constraints_placements_holder[this->m_current_constraints_pb_id];\n }\n\n /// \\brief Const getter for contact placements.\n const PlacementVector & frictional_point_constraints_placements() const\n {\n return this->m_frictional_point_constraints_placements_holder[this->m_current_constraints_pb_id];\n }\n\n /// \\brief Getter for contact placements at the previous time step.\n PlacementVector & previous_frictional_point_constraints_placements()\n {\n return this->m_frictional_point_constraints_placements_holder[1 - this->m_current_constraints_pb_id];\n }\n\n /// \\brief Const getter for contact placements at the previous time step.\n const PlacementVector & previous_frictional_point_constraints_placements() const\n {\n return this->m_frictional_point_constraints_placements_holder[1 - this->m_current_constraints_pb_id];\n }\n\n /// \\brief Collecting active set from the solution of the contact problem.\n /// the contact problem should be solved before calling this method.\n void collectActiveSet(Scalar epsilon = 1e-6);\n };\n\n} // namespace simple\n\n/* --- Details -------------------------------------------------------------- */\n#include \"simple/core/constraints-problem.hxx\"\n\n#if SIMPLE_ENABLE_TEMPLATE_INSTANTIATION\n #include \"simple/core/constraints-problem.txx\"\n#endif\n\n#endif // ifndef __simple_core_constraints_problem_hpp__\n" }, { "path": "include/simple/core/constraints-problem.hxx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_constraints_problem_hxx__\n#define __simple_core_constraints_problem_hxx__\n\n#include \"simple/core/constraints-problem.hpp\"\n#include \"simple/tracy.hpp\"\n#include \"simple/utils/visitors.hpp\"\n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\nnamespace simple\n{\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n ConstraintsProblemTpl::ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_point_constraint_models,\n const WeldConstraintModelVector & weld_constraint_models)\n : m_model(model_handle)\n , m_data(data_handle)\n , m_geom_model(geom_model_handle)\n , m_geom_data(geom_data_handle)\n , m_current_constraints_pb_id(0)\n , m_max_num_contact_per_collision_pair(4)\n , m_num_contacts(0)\n , m_previous_num_contacts(0)\n , m_previous_joint_limits_constraint_forces_size(0)\n {\n for (std::size_t i = 0; i < bilateral_point_constraint_models.size(); ++i)\n {\n this->bilateral_point_constraint_models.emplace_back(bilateral_point_constraint_models[i]);\n this->bilateral_point_constraint_datas.emplace_back(bilateral_point_constraint_models[i].createData());\n }\n for (std::size_t i = 0; i < weld_constraint_models.size(); ++i)\n {\n this->weld_constraint_models.emplace_back(weld_constraint_models[i]);\n this->weld_constraint_datas.emplace_back(weld_constraint_models[i].createData());\n }\n this->allocate();\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n ConstraintsProblemTpl::ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle)\n : ConstraintsProblemTpl(\n model_handle, //\n data_handle, //\n geom_model_handle, //\n geom_data_handle, //\n BilateralPointConstraintModelVector(), //\n WeldConstraintModelVector())\n {\n }\n\n template class JointCollectionTpl>\n ConstraintsProblemTpl::ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_point_constraint_models)\n : ConstraintsProblemTpl(\n model_handle, //\n data_handle, //\n geom_model_handle, //\n geom_data_handle, //\n bilateral_point_constraint_models, //\n WeldConstraintModelVector())\n {\n }\n\n template class JointCollectionTpl>\n ConstraintsProblemTpl::ConstraintsProblemTpl(\n const ModelHandle & model_handle,\n const DataHandle & data_handle,\n const GeometryModelHandle & geom_model_handle,\n const GeometryDataHandle & geom_data_handle,\n const WeldConstraintModelVector & weld_constraint_models)\n : ConstraintsProblemTpl(\n model_handle, //\n data_handle, //\n geom_model_handle, //\n geom_data_handle, //\n BilateralPointConstraintModelVector(), //\n weld_constraint_models)\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void ConstraintsProblemTpl::clear()\n {\n // Clearing joint limits\n this->m_previous_joint_limits_constraint_forces_size = this->joint_limit_constraint_size();\n JointLimitConstraintModel * jlcm = boost::get(&this->joint_limit_constraint_model);\n assert(jlcm != nullptr);\n if (jlcm != nullptr)\n {\n this->m_previous_joint_limit_active_set = jlcm->getActiveSetIndexes();\n }\n\n // Clearing contacts\n this->pairs_in_collision.clear();\n this->contact_id_to_collision_pair.clear();\n this->frictional_point_constraints_placements().clear();\n this->m_previous_num_contacts = this->m_num_contacts;\n this->m_num_contacts = 0;\n\n // Clearing constraint models corresponding to joint limits and contacts\n std::size_t begin_idx_to_erase = this->getNumberOfJointFrictionConstraints();\n begin_idx_to_erase += this->getNumberOfBilateralConstraints();\n begin_idx_to_erase += this->getNumberOfWeldConstraints();\n this->constraint_models.erase(this->constraint_models.begin() + (int)begin_idx_to_erase, this->constraint_models.end());\n this->constraint_datas.erase(this->constraint_datas.begin() + (int)begin_idx_to_erase, this->constraint_datas.end());\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void ConstraintsProblemTpl::allocate()\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"ConstraintsProblem::allocate\");\n\n //\n // We first allocate memory for the constraints by setting their holders.\n //\n\n // -- Joint friction\n // first we check which joint actually has friction:\n int size_dry_friction = 0;\n std::vector active_friction_joints_ids;\n std::vector dry_friction_joint_idx_v;\n for (std::size_t i = 1; i < (std::size_t)this->model().njoints; ++i)\n {\n const pinocchio::JointModel & joint = this->model().joints[i];\n if (\n !(this->model().upperDryFrictionLimit.segment(joint.idx_v(), joint.nv()).isZero())\n || !(this->model().lowerDryFrictionLimit.segment(joint.idx_v(), joint.nv()).isZero()))\n {\n dry_friction_joint_idx_v.push_back(size_dry_friction);\n active_friction_joints_ids.push_back(joint.id());\n size_dry_friction += joint.nv();\n }\n }\n // now we can build the holder for the frictional joint constraint\n VectorXs lowerDryFrictionLimit = VectorXs::Zero(size_dry_friction);\n VectorXs upperDryFrictionLimit = VectorXs::Zero(size_dry_friction);\n for (std::size_t i = 0; i < active_friction_joints_ids.size(); ++i)\n {\n const JointIndex joint_id = active_friction_joints_ids[i];\n const pinocchio::JointModel & joint = this->model().joints[joint_id];\n lowerDryFrictionLimit.segment(dry_friction_joint_idx_v[i], joint.nv()) =\n this->model().lowerDryFrictionLimit.segment(joint.idx_v(), joint.nv());\n upperDryFrictionLimit.segment(dry_friction_joint_idx_v[i], joint.nv()) =\n this->model().upperDryFrictionLimit.segment(joint.idx_v(), joint.nv());\n }\n\n FrictionalJointConstraintModel fjcm(this->model(), active_friction_joints_ids);\n fjcm.set() = BoxSet(lowerDryFrictionLimit, upperDryFrictionLimit);\n this->joint_friction_constraint_model = fjcm;\n this->joint_friction_constraint_data = fjcm.createData();\n\n // -- Bilateral\n // they have been filled in the constructor\n\n // -- Weld\n // they have been filled in the constructor\n\n // -- Joint limits\n // first we check which joint actually has limits:\n std::vector active_limit_joints_ids;\n for (std::size_t i = 1; i < (std::size_t)this->model().njoints; ++i)\n {\n const pinocchio::JointModel & joint = this->model().joints[i];\n if (joint.nq() == 1 && joint.hasConfigurationLimit()[0])\n {\n active_limit_joints_ids.push_back(joint.id());\n }\n // TODO: understand why we need to force nq==1\n // for (std::size_t j = 0; j < (std::size_t)joint.nq(); ++j)\n // {\n // bool has_limit = joint.hasConfigurationLimit()[j];\n // if (has_limit)\n // {\n // active_limit_joints_ids.push_back(joint.id());\n // break;\n // }\n // }\n }\n // now we can build the holder for the joint limits constraints\n // TODO check that we use the margins contained in the model\n JointLimitConstraintModel jlcm(this->model(), active_limit_joints_ids);\n this->joint_limit_constraint_model = jlcm;\n this->joint_limit_constraint_data = jlcm.createData();\n this->m_previous_joint_limit_active_set.reserve(std::size_t(this->joint_limit_constraint_max_size()));\n\n // -- Frictional contacts\n // For now, it's assumed that there is only one `hpp::fcl::Contact` per collision pair, meaning\n // that although there can be multiple contact points between two shapes, there can only be one\n // normal (and thus one contact patch for each collision pair). Note: each `Contact` has its own\n // `ContactPatch`. In the future, if we support multiple contact normals for a single collision\n // pair (i.e. for two non-convex objects), we will have to recompute `m_max_num_contacts`, by\n // factoring the number of `Contact` per collision pair.\n assert(this->geom_data().collisionRequests.size() == this->geom_model().collisionPairs.size());\n assert(this->geom_data().collisionRequests.size() == this->geom_data().collisionResults.size());\n std::size_t max_num_contacts = this->geom_model().collisionPairs.size() * this->m_max_num_contact_per_collision_pair;\n const int frictional_contact_max_size = (int)(3 * max_num_contacts);\n\n this->pairs_in_collision.reserve(this->geom_model().collisionPairs.size());\n this->contact_id_to_collision_pair.reserve(max_num_contacts);\n this->frictional_point_constraints_placements().reserve(max_num_contacts);\n this->previous_frictional_point_constraints_placements().reserve(max_num_contacts);\n this->previous_colliding_collision_pairs.assign(this->geom_model().collisionPairs.size(), false);\n this->contact_mappers.assign(this->geom_model().collisionPairs.size(), ContactMapper(0, 0));\n this->breaking_contacts.reserve(max_num_contacts);\n this->sticking_contacts.reserve(max_num_contacts);\n this->sliding_contacts.reserve(max_num_contacts);\n this->contact_modes.reserve(max_num_contacts);\n\n this->frictional_point_constraint_models.reserve(max_num_contacts);\n this->frictional_point_constraint_datas.reserve(max_num_contacts);\n this->frictional_point_constraint_models.clear();\n this->frictional_point_constraint_datas.clear();\n const ::pinocchio::FrictionCoefficientMatrix & friction_coefficients = ::pinocchio::getFrictionCoefficientMatrix();\n for (::pinocchio::PairIndex col_pair_id = 0; col_pair_id < this->geom_model().collisionPairs.size(); ++col_pair_id)\n {\n // add contact models/datas\n const GeomIndex geom_id1 = this->geom_model().collisionPairs[col_pair_id].first;\n const GeomIndex geom_id2 = this->geom_model().collisionPairs[col_pair_id].second;\n\n const ::pinocchio::GeometryObject & geom1 = this->geom_model().geometryObjects[geom_id1];\n const ::pinocchio::GeometryObject & geom2 = this->geom_model().geometryObjects[geom_id2];\n\n const ::pinocchio::JointIndex joint_id1 = geom1.parentJoint;\n const ::pinocchio::JointIndex joint_id2 = geom2.parentJoint;\n\n // fill collision pairs frictions and elasticities\n const ::pinocchio::PhysicsMaterial & material1 = geom1.physicsMaterial;\n const ::pinocchio::PhysicsMaterial & material2 = geom2.physicsMaterial;\n\n const Scalar friction =\n static_cast(friction_coefficients.getFrictionFromMaterialPair(material1.materialType, material2.materialType));\n const Scalar compliance = static_cast(Scalar(0.5) * (material1.compliance + material2.compliance));\n // const Scalar elasticity = static_cast(0.5 * (material1.elasticity + material2.elasticity));\n\n for (ContactIndex contact_id = 0; contact_id < this->m_max_num_contact_per_collision_pair; ++contact_id)\n {\n FrictionalPointConstraintModel fpcm(this->model(), joint_id1, SE3::Identity(), joint_id2, SE3::Identity());\n const std::string name = \"FrictionalPointConstraintModel_\" + std::to_string(geom_id1) + \"_\" + std::to_string(geom_id2);\n fpcm.name = name;\n fpcm.compliance().setConstant(compliance);\n fpcm.desired_constraint_offset = Vector3s(0, 0, this->geom_data().collisionRequests[col_pair_id].security_margin);\n fpcm.set() = CoulombFrictionCone(friction);\n this->frictional_point_constraint_models.emplace_back(fpcm);\n this->frictional_point_constraint_models.back().name = name;\n this->frictional_point_constraint_datas.emplace_back(fpcm.createData());\n }\n }\n assert(this->frictional_point_constraint_models.size() == max_num_contacts);\n assert(this->frictional_point_constraint_datas.size() == max_num_contacts);\n\n //\n // Now that all constraints have been listed, we can allocate the maximum memory for the constraint models and datas references.\n // These references will point to the data stored in the constraints models/data that are stored in the holders.\n // Certain constraints like frictional point constraints are dynamic, meaning that at a certain\n // time step they can be active or inactive.\n // Hence, after these constraints have been added to constraint_models/datas and that memory\n // has been allocated, they are removed from constraint_models/data.\n // It's then the job of the `update` method to deal with dynamic constraints.\n //\n std::size_t max_num_constraints = max_num_contacts + this->bilateral_point_constraint_models.size();\n max_num_constraints += this->weld_constraint_models.size();\n // TODO this is wrong, we can have more than one joint friction constraint or one joint limit constraint\n max_num_constraints += this->joint_friction_constraint_size() > 0 ? 1 : 0;\n max_num_constraints += this->joint_limit_constraint_size() > 0 ? 1 : 0;\n this->constraint_models.reserve(max_num_constraints);\n this->constraint_datas.reserve(max_num_constraints);\n this->constraint_models.clear();\n this->constraint_datas.clear();\n\n // variable used to make sure we allocate enough data to hold any constraint jacobian\n int max_constraint_size = this->geom_model().collisionPairs.size() > 0 ? 3 : 0;\n if (this->joint_friction_constraint_model.size() > 0)\n {\n this->constraint_models.emplace_back(this->joint_friction_constraint_model);\n this->constraint_datas.emplace_back(this->joint_friction_constraint_data);\n max_constraint_size = std::max(this->joint_friction_constraint_size(), max_constraint_size);\n }\n if (this->bilateral_point_constraint_models.size() > 0)\n {\n for (std::size_t i = 0; i < this->bilateral_point_constraint_models.size(); ++i)\n {\n this->constraint_models.emplace_back(this->bilateral_point_constraint_models[i]);\n this->constraint_datas.emplace_back(this->bilateral_point_constraint_datas[i]);\n }\n max_constraint_size = std::max(3, max_constraint_size);\n }\n if (this->weld_constraint_models.size() > 0)\n {\n for (std::size_t i = 0; i < this->weld_constraint_models.size(); ++i)\n {\n this->constraint_models.emplace_back(this->weld_constraint_models[i]);\n this->constraint_datas.emplace_back(this->weld_constraint_datas[i]);\n }\n max_constraint_size = std::max(6, max_constraint_size);\n }\n if (this->joint_limit_constraint_model.size() > 0)\n {\n this->constraint_models.emplace_back(this->joint_limit_constraint_model);\n this->constraint_datas.emplace_back(this->joint_limit_constraint_data);\n max_constraint_size = std::max(this->joint_limit_constraint_max_size(), max_constraint_size);\n }\n if (this->frictional_point_constraint_models.size() > 0)\n {\n for (std::size_t i = 0; i < this->frictional_point_constraint_models.size(); ++i)\n {\n this->constraint_models.emplace_back(this->frictional_point_constraint_models[i]);\n this->constraint_datas.emplace_back(this->frictional_point_constraint_datas[i]);\n }\n max_constraint_size = std::max(3, max_constraint_size);\n }\n\n const int constraints_problem_max_size = this->joint_friction_constraint_size() //\n + this->bilateral_constraints_size() //\n + this->weld_constraints_size() //\n + this->joint_limit_constraint_max_size() //\n + frictional_contact_max_size; //\n\n // allocate the rest of the ConstraintsProblem\n this->constraint_cholesky_decomposition.resize(this->model(), this->constraint_models);\n this->m_g.resize(constraints_problem_max_size);\n this->m_preconditioner.resize(constraints_problem_max_size);\n this->m_time_scaling_acc_to_constraints.resize(constraints_problem_max_size);\n this->m_constraints_forces_holder[0].resize(constraints_problem_max_size);\n this->m_constraints_forces_holder[1].resize(constraints_problem_max_size);\n this->m_constraints_velocities_warmstarts.resize(constraints_problem_max_size);\n this->m_constraints_velocities.resize(constraints_problem_max_size);\n\n // initialize constraints forces\n this->constraints_forces().setZero();\n this->previous_constraints_forces().setZero();\n\n this->clear(); // remove frictional contact models from constraints vector\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void ConstraintsProblemTpl::update(const bool compute_warm_start)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"ConstraintsProblem::update\");\n\n this->updateConstraintsHolders();\n this->clear();\n assert(this->m_num_contacts == 0 && \"The number of contacts should be 0.\");\n assert(this->geom_data().collisionResults.size() == geom_model().collisionPairs.size());\n assert(this->checkConstraintsConsistencyWithGeometryModel());\n\n // Process joint limit constraints to check which ones are active\n ConstraintModel & constraint_model = this->joint_limit_constraint_model;\n ConstraintData & constraint_data = this->joint_limit_constraint_data;\n JointLimitConstraintModel * jlcm = boost::get(&constraint_model);\n assert(jlcm != nullptr);\n if (jlcm != nullptr)\n {\n JointLimitConstraintData * jlcd = boost::get(&constraint_data);\n assert(jlcd != nullptr);\n jlcm->resize(this->model(), this->data(), *jlcd);\n }\n constraint_model.calc(this->model(), this->data(), constraint_data);\n int active_size = constraint_model.activeSize();\n if (active_size > 0)\n {\n this->joint_limit_constraint_forces().setZero();\n if (compute_warm_start && this->previous_joint_limit_constraint_size() > 0)\n {\n // Warm-start the joint limits constraint forces.\n // We assume that the active set indexes are sorted.\n // We search in the current active set which index belongs to the previous active set.\n const std::vector & prev_active_set = this->m_previous_joint_limit_active_set;\n assert(prev_active_set.empty() == false);\n const std::vector & active_set = jlcm->getActiveSetIndexes();\n\n std::size_t j = 0;\n for (std::size_t i = 0; i < active_set.size(); ++i)\n {\n const std::size_t idx_active = active_set[i];\n for (; j < prev_active_set.size(); ++j)\n {\n const std::size_t idx_prev_active = prev_active_set[j];\n if (idx_prev_active > idx_active)\n {\n break;\n }\n if (idx_prev_active == idx_active)\n {\n this->joint_limit_constraint_forces()[int(i)] = this->previous_joint_limit_constraint_forces()[int(j)];\n break;\n }\n }\n }\n }\n this->constraint_models.emplace_back(constraint_model);\n this->constraint_datas.emplace_back(constraint_data);\n }\n\n ContactIndex col_pair_contact_model_begin_id = 0; // Tracks the id of the first constraint model of the current collision pair\n for (GeomIndex col_pair_id = 0; col_pair_id < this->geom_model().collisionPairs.size(); col_pair_id++)\n {\n // TODO(louis): check if pair is active\n const hpp::fcl::CollisionResult & col_res = this->geom_data().collisionResults[col_pair_id];\n const hpp::fcl::ContactPatchResult & patch_res = this->geom_data().contactPatchResults[col_pair_id];\n // TODO(louis): extend to deal with multiple contact patches per collision pair.\n if (col_res.isCollision() && patch_res.numContactPatches() > 0)\n {\n this->pairs_in_collision.emplace_back(col_pair_id);\n\n const GeomIndex geom_id1 = this->geom_model().collisionPairs[col_pair_id].first;\n const GeomIndex geom_id2 = this->geom_model().collisionPairs[col_pair_id].second;\n\n const ::pinocchio::GeometryObject & geom1 = this->geom_model().geometryObjects[geom_id1];\n const ::pinocchio::GeometryObject & geom2 = this->geom_model().geometryObjects[geom_id2];\n\n const JointIndex joint_id1 = geom1.parentJoint;\n const JointIndex joint_id2 = geom2.parentJoint;\n\n const SE3 & oMi1 = this->data().oMi[joint_id1];\n const SE3 & oMi2 = this->data().oMi[joint_id2];\n\n // For the moment, we consider only one contact patch per collision pair.\n // All the contact points of the patch share the same normal.\n const hpp::fcl::ContactPatch & patch = patch_res.getContactPatch(0);\n const Vector3s & contact_normal = patch.getNormal();\n const std::size_t num_contacts_colpair = std::min(patch.size(), this->m_max_num_contact_per_collision_pair);\n // All the points of the patch share the same normal; the rotation part of the contact frame\n // does not need to be recomputed for every point of the patch.\n SE3 oMc;\n PlacementFromNormalAndPosition::calc(contact_normal, Vector3s::Zero(), oMc);\n for (ContactIndex contact_id = 0; contact_id < num_contacts_colpair; ++contact_id)\n {\n this->contact_id_to_collision_pair.push_back(col_pair_id);\n\n //\n oMc.translation() = patch.getPoint(contact_id);\n this->frictional_point_constraints_placements().emplace_back(oMc);\n const SE3 i1Mc = oMi1.actInv(oMc);\n //\n SE3 & oMc1 = oMc;\n oMc1.translation() = patch.getPointShape1(contact_id);\n const SE3 i1Mc1 = oMi1.actInv(oMc1);\n //\n SE3 & oMc2 = oMc;\n oMc2.translation() = patch.getPointShape2(contact_id);\n const SE3 i2Mc2 = oMi2.actInv(oMc2);\n\n // Update constraint models and datas\n const std::size_t constraint_model_id = col_pair_contact_model_begin_id + contact_id;\n FrictionalPointConstraintModel * fpcmodel =\n boost::get(&(this->frictional_point_constraint_models[constraint_model_id]));\n if (fpcmodel == nullptr)\n {\n PINOCCHIO_THROW_PRETTY(std::runtime_error, \"Invalid constraint model type.\");\n }\n else\n {\n fpcmodel->joint1_placement = i1Mc1;\n fpcmodel->joint2_placement = i2Mc2;\n }\n // Note: friction is set in `allocate` when constructing the frictional point constraints.\n // The friction is retrieved from the materials of the geometries.\n // The same can be done for elasticity.\n const ConstraintModel & constraint_model = this->frictional_point_constraint_models[constraint_model_id];\n ConstraintData & constraint_data = this->frictional_point_constraint_datas[constraint_model_id];\n this->constraint_models.emplace_back(constraint_model);\n this->constraint_datas.emplace_back(constraint_data);\n\n constraint_model.calc(this->model(), this->data(), constraint_data);\n#ifndef NDEBUG\n // sanity check\n const ::coal::CollisionRequest & col_req = this->geom_data().collisionRequests[col_pair_id];\n const FrictionalPointConstraintData * fpcdata = boost::get(&constraint_data);\n assert(fpcdata != nullptr);\n assert(\n std::abs((fpcdata->constraint_position_error - Vector3s(0, 0, patch.penetration_depth - col_req.security_margin)).norm())\n < std::sqrt(Eigen::NumTraits::dummy_precision()));\n#endif // NDEBUG\n\n // Warm-start the contact forces\n const auto cindex = static_cast(3 * this->m_num_contacts);\n Vector3s warm_start_force = Vector3s::Zero();\n if (compute_warm_start && this->previous_colliding_collision_pairs[col_pair_id])\n {\n Scalar closest_previous_contact_distance = std::numeric_limits::infinity();\n ContactIndex closest_previous_contact_id = this->contact_mappers[col_pair_id].begin;\n SE3 cprevMc;\n for (ContactIndex previous_contact_id = this->contact_mappers[col_pair_id].begin;\n previous_contact_id < this->contact_mappers[col_pair_id].begin + this->contact_mappers[col_pair_id].count;\n ++previous_contact_id)\n {\n // TODO(quentin) search which previous contact corresponds to the current one in a more clever way\n const SE3 & i1Mc_previous = this->previous_frictional_point_constraints_placements()[previous_contact_id];\n const SE3 cprevMc_tmp = i1Mc_previous.actInv(i1Mc);\n Scalar previous_contact_distance = (i1Mc.translation() - i1Mc_previous.translation()).norm();\n if (previous_contact_distance < closest_previous_contact_distance)\n {\n closest_previous_contact_distance = previous_contact_distance;\n closest_previous_contact_id = previous_contact_id;\n cprevMc = cprevMc_tmp;\n }\n }\n const auto cprev_index = static_cast(3 * closest_previous_contact_id);\n warm_start_force.noalias() = cprevMc.rotation()\n * this->previous_frictional_point_constraints_forces().template segment<3>(\n cprev_index); // TODO(quentin) check this is properly done\n }\n // Important: first update the number of contact, then set the eigen vectors like\n // frictional_point_constraints_forces, in this order.\n ++(this->m_num_contacts);\n this->frictional_point_constraints_forces().template segment<3>(cindex) = warm_start_force;\n }\n // update previous_colliding_collision_pairs and m_contact_mappers with newly created contacts\n this->previous_colliding_collision_pairs[col_pair_id] = true;\n\n // The role of the contact mapper is to map a collision pair to its number of contact points.\n this->contact_mappers[col_pair_id].begin = this->m_num_contacts - num_contacts_colpair;\n this->contact_mappers[col_pair_id].count = num_contacts_colpair;\n }\n else\n {\n this->contact_mappers[col_pair_id].begin = 0;\n this->contact_mappers[col_pair_id].count = 0; // nothing to map.\n this->previous_colliding_collision_pairs[col_pair_id] = false;\n }\n col_pair_contact_model_begin_id += this->m_max_num_contact_per_collision_pair;\n }\n\n#ifndef NDEBUG\n const std::size_t num_constraints = this->getNumberOfJointFrictionConstraints() //\n + this->getNumberOfBilateralConstraints() //\n + this->getNumberOfWeldConstraints() //\n + this->getNumberOfJointLimitConstraints() //\n + this->getNumberOfContacts();\n assert(this->constraint_models.size() == num_constraints);\n assert(this->constraint_datas.size() == num_constraints);\n#endif\n\n // Call calc on the remaining constraint models (it has already been called for joint limits and frictional contacts)\n for (std::size_t i = 0;\n i < this->getNumberOfJointFrictionConstraints() + this->getNumberOfBilateralConstraints() + this->getNumberOfWeldConstraints();\n ++i)\n {\n const ConstraintModel & constraint_model = this->constraint_models[i];\n ConstraintData & constraint_data = this->constraint_datas[i];\n constraint_model.calc(this->model(), this->data(), constraint_data);\n }\n\n // Update all constraints but frictional points constraints warmstarts\n if (this->joint_friction_constraint_size() > 0)\n {\n this->joint_friction_constraint_forces() = this->previous_joint_friction_constraint_forces();\n }\n\n if (this->bilateral_constraints_size() > 0)\n {\n this->bilateral_constraints_forces() = this->previous_bilateral_constraints_forces();\n }\n\n if (this->weld_constraints_size() > 0)\n {\n this->weld_constraints_forces() = this->previous_weld_constraints_forces();\n }\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n template\n void ConstraintsProblemTpl::build(\n const Eigen::MatrixBase & vfree,\n const Eigen::MatrixBase & v,\n const Eigen::MatrixBase & v_warmstart,\n S dt)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"ConstraintsProblem::build\");\n PINOCCHIO_UNUSED_VARIABLE(v_warmstart);\n\n PINOCCHIO_CHECK_ARGUMENT_SIZE(vfree.size(), v.size(), \"The free velocity and the velocity should have the same size.\");\n PINOCCHIO_CHECK_INPUT_ARGUMENT(dt >= 0, \"The time step should be positive or null.\");\n PINOCCHIO_CHECK_ARGUMENT_SIZE(\n this->constraint_models.size(), this->constraint_datas.size(), \"The number of constraint models and datas should be the same.\");\n assert(this->check());\n\n // Compute `G`, the Delassus operator\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"ConstraintsProblem::build - resize Delassus cholesky\");\n this->constraint_cholesky_decomposition.resize(this->model(), this->constraint_models);\n }\n\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"ConstraintsProblem::build - compute Delassus cholesky\");\n // TODO(quentin): replace rho\n const Scalar rho = 1e-6;\n this->constraint_cholesky_decomposition.compute(this->model(), this->data(), this->constraint_models, this->constraint_datas, rho);\n }\n\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"ConstraintsProblem::build - compute g\");\n this->computeConstraintsDrift(vfree, v, dt);\n }\n\n this->preconditioner().setOnes();\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n template\n void ConstraintsProblemTpl::computeConstraintsDrift(\n const Eigen::MatrixBase & vfree, const Eigen::MatrixBase & v, const Scalar dt)\n {\n // TODO: express everything in velocity and use pinocchio helper to go to formulation level\n\n int cindex = 0;\n for (std::size_t i = 0; i < this->constraint_models.size(); ++i)\n {\n const ConstraintModel & cmodel = this->constraint_models[i];\n const ConstraintData & cdata = this->constraint_datas[i];\n const int cdim = cmodel.activeSize();\n auto gc = this->g().segment(cindex, cdim);\n\n using MapVectorXs = Eigen::Map;\n auto drift_visitor = ::simple::visitors::make_lambda_visitor(\n //\n // Visitor for FrictionalJointConstraintModel - velocity formulation\n [&](const FrictionalJointConstraintModel & cmodel) {\n const FrictionalJointConstraintData & cdata_ = boost::get(cdata);\n cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vfree, gc);\n },\n //\n // Visitor for BilateralConstraintModel - velocity formulation\n [&](const BilateralPointConstraintModel & cmodel) {\n const BilateralPointConstraintData & cdata_ = boost::get(cdata);\n const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp;\n const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd;\n\n // -- Jc * (vfree - kd * v)\n MapVectorXs vtmp(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1));\n vtmp = vfree - kd * v;\n cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmp, gc);\n\n // -- baumgarte kp correction\n const Vector3s correction = kp * cdata_.constraint_position_error;\n gc += correction / dt;\n },\n //\n // Visitor for WeldConstraintModel - velocity formulation\n [&](const WeldConstraintModel & cmodel) {\n const WeldConstraintData & cdata_ = boost::get(cdata);\n const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp;\n const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd;\n\n // -- Jc * (vfree - kd * v)\n MapVectorXs vtmp(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1));\n vtmp = vfree - kd * v;\n cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmp, gc);\n\n // -- baumgaarte kp correction\n const Vector6s correction = kp * cdata_.constraint_position_error;\n gc += correction / dt;\n },\n //\n // Visitor for JointLimitConstraintModel - position formulation\n [&](const JointLimitConstraintModel & cmodel) {\n const JointLimitConstraintData & cdata_ = boost::get(cdata);\n const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp;\n const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd;\n\n // -- Jc * (vfree - kd * v) * dt\n MapVectorXs vtmpdt(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1));\n vtmpdt = (vfree - kd * v) * dt;\n cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmpdt, gc);\n\n // -- baumgarte kp correction\n MapVectorXs joint_limit_correction(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, cmodel.activeSize(), 1));\n joint_limit_correction = cdata_.constraint_residual; // copy\n\n const auto lower_bound_size = cmodel.set().getNegativeOrthant().size();\n auto joint_limit_correction_lower = joint_limit_correction.head(lower_bound_size);\n joint_limit_correction_lower = (joint_limit_correction_lower.array() > 0) // if\n .select( //\n joint_limit_correction_lower.array() * kp, // then\n joint_limit_correction_lower.array()); // else\n\n const auto upper_bound_size = cmodel.set().getPositiveOrthant().size();\n auto joint_limit_correction_upper = joint_limit_correction.tail(upper_bound_size);\n joint_limit_correction_upper = (joint_limit_correction_upper.array() < 0) // if\n .select( //\n joint_limit_correction_upper.array() * kp, // then\n joint_limit_correction_upper.array()); // else\n\n gc += joint_limit_correction;\n },\n //\n // Visitor for FrictionalPointConstraintModel - velocity formulation\n [&](const FrictionalPointConstraintModel & cmodel) {\n const FrictionalPointConstraintData & cdata_ = boost::get(cdata);\n const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp;\n const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd;\n\n // TODO(quentin): no magic forces ?\n // TODO(quentin): take security_margin into account\n // TODO(louis): deal with elasticity. For that, we need to register which contact pairs were\n // in contact at the previous time step.\n // Vector3s correction(0, 0, std::min(0, Kb * penetration_depth / dt + elasticity * Jcv.coeff(2)));\n // -- Jc * (vfree - kd * v)\n MapVectorXs vtmp(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1));\n vtmp = vfree - kd * v;\n cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmp, gc);\n\n // -- baumgarte kp correction\n Vector3s correction = cdata_.constraint_position_error;\n correction = (correction.array() < 0) // if\n .select( //\n correction.array() * kp, // then\n 0.); // else\n gc += correction / dt;\n });\n\n boost::apply_visitor(drift_visitor, cmodel);\n\n cindex += cdim;\n }\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void ConstraintsProblemTpl::updateConstraintsHolders()\n {\n this->m_current_constraints_pb_id = 1 - this->m_current_constraints_pb_id;\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n bool ConstraintsProblemTpl::checkConstraintsConsistencyWithGeometryModel() const\n {\n assert(\n this->geom_data().collisionRequests.size() == this->geom_model().collisionPairs.size() //\n && \"geom_data and geom_model do not coincide.\");\n assert(\n this->geom_data().collisionRequests.size() == this->geom_data().collisionResults.size() //\n && \"geom_data is inconsistent.\");\n\n std::size_t max_num_contacts = this->geom_model().collisionPairs.size() * this->m_max_num_contact_per_collision_pair;\n\n return (\n this->frictional_point_constraint_models.size() == max_num_contacts //\n && this->frictional_point_constraint_datas.size() == max_num_contacts); //\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void ConstraintsProblemTpl::collectActiveSet(Scalar epsilon)\n {\n // TODO we should add the active set for the other constraint types.\n this->breaking_contacts.clear();\n this->sticking_contacts.clear();\n this->sliding_contacts.clear();\n this->contact_modes.clear();\n\n const Vector3s e_z(0, 0, Scalar(1));\n const auto nc = static_cast(this->getNumberOfContacts());\n for (Eigen::Index contact_id = 0; contact_id < nc; ++contact_id)\n {\n const Vector3s lambda_i = this->frictional_point_constraints_forces().template segment<3>(3 * contact_id);\n // TODO(quentinll) check if the compliance is taken into account in sigma_i. It should be.\n const Vector3s sigma_i = this->frictional_point_constraints_velocities().template segment<3>(3 * contact_id);\n if (lambda_i.norm() < epsilon) // breaking\n {\n this->breaking_contacts.push_back((ContactIndex)contact_id);\n this->contact_modes.push_back(ContactMode::BREAKING);\n }\n else if (sigma_i.norm() < epsilon) // sticking\n {\n this->sticking_contacts.push_back((ContactIndex)contact_id);\n this->contact_modes.push_back(ContactMode::STICKING);\n }\n else // sliding\n {\n this->sliding_contacts.push_back((ContactIndex)contact_id);\n this->contact_modes.push_back(ContactMode::SLIDING);\n }\n }\n\n assert(this->contact_modes.size() == (std::size_t)nc && \"Wrong size of contact modes vector.\");\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n bool ConstraintsProblemTpl::check() const\n {\n const auto problem_size = static_cast(this->constraints_problem_size());\n assert(this->m_model != nullptr && \"The model handle points to nullptr.\");\n assert(this->m_data != nullptr && \"The data handle points to nullptr.\");\n assert(this->m_geom_model != nullptr && \"The geometry model handle points to nullptr.\");\n assert(this->m_geom_data != nullptr && \"The geometry data handle points to nullptr.\");\n const std::size_t num_constraints = this->getNumberOfJointFrictionConstraints() //\n + this->getNumberOfBilateralConstraints() //\n + this->getNumberOfWeldConstraints() //\n + this->getNumberOfJointLimitConstraints() //\n + this->getNumberOfContacts();\n assert(this->constraint_models.size() == num_constraints);\n assert(this->constraint_datas.size() == num_constraints);\n assert(\n (this->m_g.size() == this->m_constraints_velocities_warmstarts.size()) //\n && \"g and constraint should always have the same (maximum) size.\");\n assert((this->m_g.size() >= problem_size) && \"The size of m_g is too small to contain the contact problem.\");\n assert((this->g().size() == problem_size) && \"The size of g does not match the size of the contact problem.\");\n assert(\n (this->frictional_point_constraints_warmstart().size() == this->frictional_point_constraints_size())\n && \"The size of the contact velocity warm-start does not match the size of the contact \"\n \"problem.\");\n assert(\n (this->checkConstraintsConsistencyWithGeometryModel())\n && \"The constraints model and data vectors are not consistent with the geom_model and the \"\n \"geom_data.\");\n\n return static_cast(\n this->m_model != nullptr //\n && this->m_data != nullptr //\n && this->m_geom_model != nullptr //\n && this->m_geom_data != nullptr //\n && this->constraint_models.size() == num_constraints //\n && this->constraint_datas.size() == num_constraints //\n && this->m_g.size() == this->m_constraints_velocities_warmstarts.size() //\n && this->m_g.size() >= problem_size //\n && this->g().size() == problem_size //\n && this->frictional_point_constraints_warmstart().size() == this->frictional_point_constraints_size() //\n && this->checkConstraintsConsistencyWithGeometryModel());\n }\n\n} // namespace simple\n\n#endif // __simple_core_constraints_problem_hxx__\n" }, { "path": "include/simple/core/constraints-problem.txx", "content": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#ifndef __simple_core_constraints_problem_txx__\n#define __simple_core_constraints_problem_txx__\n\n#include \"simple/core/constraints-problem.hpp\"\n\nnamespace simple\n{\n\n extern template struct ConstraintsProblemTpl;\n\n} // namespace simple\n\n#endif // __simple_core_constraints_problem_txx__\n" }, { "path": "include/simple/core/contact-frame.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_contact_frame_hpp__\n#define __simple_core_contact_frame_hpp__\n\n#include \"simple/core/fwd.hpp\"\n\n#include \n#include \n\n#ifndef SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS\n #include \n#endif\n\nnamespace simple\n{\n\n /// @brief Operator to construct a placement from a given normal and position.\n template\n struct PlacementFromNormalAndPositionTpl\n {\n using Scalar = _Scalar;\n enum\n {\n Options = _Options\n };\n\n using SE3 = ::pinocchio::SE3Tpl;\n using Matrix6x3s = Eigen::Matrix;\n using Vector3s = Eigen::Matrix;\n\n /// \\brief Computes a placement M from a normal and position.\n /// \\param[in] normal is the z-axis of the rotation part of M.\n /// \\param[in] position is the translation part of M.\n /// \\param[out] M the placement.\n template\n static void calc(\n const Eigen::MatrixBase & normal, // [input]\n const Eigen::MatrixBase & position, // [input]\n SE3 & M // [output]\n );\n\n /// \\brief Computes the derivative of M w.r.t normal and position used to construct it.\n /// \\param[in] M\n /// \\param[out] dM_dnormal derivative w.r.t normal\n /// \\param[out] dM_dposition derivative w.r.t position\n template\n static void calcDiff(\n const SE3 & M, // [input]\n const Eigen::MatrixBase & dM_dnormal, // [output]\n const Eigen::MatrixBase & dM_dposition // [output]\n );\n\n protected:\n /// \\brief Given a normal, returns the associated non-coliear reference vector.\n /// This vector is then used to compute the rotation part of the placement.\n template\n static Vector3s getReferenceVector(const Eigen::MatrixBase & normal)\n {\n EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);\n\n Vector3s e_ref(PlacementFromNormalAndPosition::reference_vector());\n static constexpr Scalar eps = 1e-6;\n if ((e_ref.cross(normal)).norm() <= eps)\n {\n e_ref = PlacementFromNormalAndPosition::other_reference_vector();\n }\n\n return e_ref;\n }\n\n /// \\brief Reference vector to compute the rotation part of a placement.\n /// If the normal is colinear to this reference vector, `e_ref2` is used instead.\n static Vector3s reference_vector()\n {\n return {1, 0, 0};\n }\n\n /// \\brief Other reference vector to compute the rotation part of a placement.\n static Vector3s other_reference_vector()\n {\n return {0, 1, 0};\n }\n };\n\n#ifndef SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS\n /// \\brief Functor to compute the derivatives of a contact patch.\n struct ComputeContactPatchDerivative : ::diffcoal::ComputeContactPatchDerivative\n {\n using Base = ::diffcoal::ComputeContactPatchDerivative;\n using GeometryObject = ::pinocchio::GeometryObject;\n\n ComputeContactPatchDerivative(const GeometryObject & go1, const GeometryObject & go2)\n : Base(go1.geometry.get(), go2.geometry.get())\n , go1_ptr(&go1)\n , go2_ptr(&go2)\n {\n }\n\n virtual ~ComputeContactPatchDerivative() {};\n\n virtual void run(\n const ::hpp::fcl::Transform3f & M1,\n const ::hpp::fcl::Transform3f & M2,\n const ::hpp::fcl::ContactPatch & patch,\n const ::diffcoal::ContactPatchDerivativeRequest & drequest,\n ::diffcoal::ContactPatchDerivative & dpatch) const override\n {\n typedef ::hpp::fcl::CollisionGeometry const * Pointer;\n const_cast(Base::geom1) = go1_ptr->geometry.get();\n const_cast(Base::geom2) = go2_ptr->geometry.get();\n return Base::run(M1, M2, patch, drequest, dpatch);\n }\n\n bool operator==(const ComputeContactPatchDerivative & other) const\n {\n return Base::operator==(other) && go1_ptr == other.go1_ptr && go2_ptr == other.go2_ptr; // Maybe, it would be better to just check\n // *go2_ptr == *other.go2_ptr\n }\n\n bool operator!=(const ComputeContactPatchDerivative & other) const\n {\n return !(*this == other);\n }\n\n const GeometryObject & getGeometryObject1() const\n {\n return *go1_ptr;\n }\n const GeometryObject & getGeometryObject2() const\n {\n return *go2_ptr;\n }\n\n protected:\n const GeometryObject * go1_ptr;\n const GeometryObject * go2_ptr;\n };\n#endif\n\n} // namespace simple\n\n#include \"simple/core/contact-frame.hxx\"\n\n#endif // __simple_core_contact_frame_hpp__\n" }, { "path": "include/simple/core/contact-frame.hxx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_contact_frame_hxx__\n#define __simple_core_contact_frame_hxx__\n\nnamespace simple\n{\n\n // ==========================================================================\n template\n template\n void PlacementFromNormalAndPositionTpl::calc(\n const Eigen::MatrixBase & normal, // [input]\n const Eigen::MatrixBase & position, // [input]\n SE3 & M // [output]\n )\n {\n EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Normal, 3);\n EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Position, 3);\n M.translation() = position;\n assert(normal.norm() - 1 <= 1e-12);\n\n // The normal will serve as the z-axis of M's rotation.\n const Vector3s normal_ = normal.normalized();\n const Vector3s e_ref(PlacementFromNormalAndPosition::getReferenceVector(normal_));\n\n M.rotation().col(2) = normal_;\n M.rotation().col(0) = (e_ref.cross(normal)).normalized();\n M.rotation().col(1) = (M.rotation().col(2)).cross(M.rotation().col(0));\n }\n\n // ========================================================================\n template\n template\n void PlacementFromNormalAndPositionTpl::calcDiff(\n const SE3 & M, // [input]\n const Eigen::MatrixBase & dM_dnormal_, // [output]\n const Eigen::MatrixBase & dM_dposition_ // [output]\n )\n {\n EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6x3Type1, 6, 3);\n EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6x3Type1, 6, 3);\n Matrix6x3Type1 & dM_dnormal = const_cast(dM_dnormal_.derived());\n Matrix6x3Type2 & dM_dposition = const_cast(dM_dposition_.derived());\n\n // TODO(louis): exploit sparsity, don't use 6x3 matrices...\n // -> dM_dn linear part is always 0\n // -> dM_dp angular part is always 0\n\n ///\n dM_dnormal.setZero();\n using Block3 = Eigen::Block;\n Block3 dM_dnormal_angular = dM_dnormal.template bottomRows<3>();\n dM_dnormal_angular.row(0) = -M.rotation().col(1);\n dM_dnormal_angular.row(1) = M.rotation().col(0);\n const Vector3s e_ref(PlacementFromNormalAndPosition::getReferenceVector(M.rotation().col(2)));\n dM_dnormal_angular.row(2) = (M.rotation().col(1)).cross(e_ref) / (e_ref.cross(M.rotation().col(2))).norm();\n\n ///\n dM_dposition.setZero();\n dM_dposition.template topRows<3>() = M.rotation().transpose();\n }\n\n} // namespace simple\n\n#endif // __simple_core_contact_frame_hxx__\n" }, { "path": "include/simple/core/fwd.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_fwd_hpp__\n#define __simple_core_fwd_hpp__\n\n#include \"simple/fwd.hpp\"\n#include \n\nnamespace simple\n{\n namespace helper\n {\n using pinocchio::helper::get_pointer;\n using pinocchio::helper::get_ref;\n } // namespace helper\n\n template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>\n struct ConstraintsProblemTpl;\n typedef ConstraintsProblemTpl ConstraintsProblem;\n\n template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>\n struct SimulatorTpl;\n typedef SimulatorTpl Simulator;\n\n template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>\n struct ContactSolverDerivativesTpl;\n typedef ContactSolverDerivativesTpl ContactSolverDerivatives;\n\n template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>\n struct SimulatorDerivativesTpl;\n typedef SimulatorDerivativesTpl SimulatorDerivatives;\n\n template\n struct PlacementFromNormalAndPositionTpl;\n typedef PlacementFromNormalAndPositionTpl PlacementFromNormalAndPosition;\n\n} // namespace simple\n\n#endif // ifndef __simple_core_fwd_hpp__\n" }, { "path": "include/simple/core/simulator.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_simulator_hpp__\n#define __simple_core_simulator_hpp__\n\n#include \"simple/core/fwd.hpp\"\n#include \"simple/core/constraints-problem.hpp\"\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n#include \n\nnamespace simple\n{\n\n template class JointCollectionTpl>\n struct traits>\n {\n using Scalar = _Scalar;\n };\n\n template class JointCollectionTpl>\n struct SimulatorTpl // : ::pinocchio::NumericalBase>\n {\n // TODO: template by allocator\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n // Typedefs\n using Scalar = _Scalar;\n enum\n {\n Options = _Options\n };\n\n using GeomIndex = pinocchio::GeomIndex;\n\n using VectorXs = Eigen::Matrix;\n using Vector3s = Eigen::Matrix;\n using Vector6s = Eigen::Matrix;\n using MatrixXs = Eigen::Matrix;\n\n using Model = ::pinocchio::ModelTpl;\n using ModelHandle = std::shared_ptr;\n using Data = typename Model::Data;\n using DataHandle = std::shared_ptr;\n using GeometryModel = ::pinocchio::GeometryModel;\n using GeometryModelHandle = std::shared_ptr;\n using GeometryData = ::pinocchio::GeometryData;\n using GeometryDataHandle = std::shared_ptr;\n\n using Force = typename Data::Force;\n using Motion = typename Data::Motion;\n using SE3 = typename Data::SE3;\n using JointModel = ::pinocchio::JointModelTpl;\n\n // TODO: template simulator by broad phase manager\n using BroadPhaseManager = ::pinocchio::BroadPhaseManagerTpl;\n using BroadPhaseManagerHandle = std::shared_ptr;\n using CollisionCallbackCollect = ::pinocchio::CollisionCallBackCollect;\n\n using ADMMConstraintSolver = ::pinocchio::ADMMContactSolverTpl;\n using PGSConstraintSolver = ::pinocchio::PGSContactSolverTpl;\n\n using SpatialForce = ::pinocchio::ForceTpl;\n using SpatialForceVector = PINOCCHIO_ALIGNED_STD_VECTOR(SpatialForce);\n\n using ConstraintsProblem = ConstraintsProblemTpl;\n using ConstraintsProblemHandle = std::shared_ptr;\n\n using ConstraintCholeskyDecomposition = typename ConstraintsProblem::ConstraintCholeskyDecomposition;\n using DelassusOperator = ::pinocchio::DelassusCholeskyExpressionTpl;\n\n using FrictionalPointConstraintModel = typename ConstraintsProblem::FrictionalPointConstraintModel;\n using FrictionalJointConstraintModel = typename ConstraintsProblem::FrictionalJointConstraintModel;\n using JointLimitConstraintModel = typename ConstraintsProblem::JointLimitConstraintModel;\n using BilateralPointConstraintModel = typename ConstraintsProblem::BilateralPointConstraintModel;\n using BilateralPointConstraintData = typename ConstraintsProblem::BilateralPointConstraintData;\n using BilateralPointConstraintModelVector = typename ConstraintsProblem::BilateralPointConstraintModelVector;\n using WeldConstraintModel = typename ConstraintsProblem::WeldConstraintModel;\n using WeldConstraintData = typename ConstraintsProblem::WeldConstraintData;\n using WeldConstraintModelVector = typename ConstraintsProblem::WeldConstraintModelVector;\n\n using ConstraintModel = typename ConstraintsProblem::ConstraintModel;\n using ConstraintData = typename ConstraintsProblem::ConstraintData;\n\n protected:\n /// \\brief Handle to the model of the system.\n ModelHandle m_model;\n\n /// \\brief Handle to the model's data of the system.\n DataHandle m_data;\n\n /// \\brief Handle to the geometry model of the system.\n GeometryModelHandle m_geom_model;\n\n /// \\brief Handle to the geometry model's data of the system.\n GeometryDataHandle m_geom_data;\n\n public:\n /// \\brief Joints configuration of the system - copied from `step` input\n VectorXs q;\n\n /// \\brief Joints velocity of the system - copied from `step` input\n VectorXs v;\n\n /// \\brief External joint torques - copied from `step` input\n VectorXs tau;\n\n /// \\brief External 6D force exerted in the local frame of each joint - copied from `step` input\n SpatialForceVector fext;\n\n /// \\brief Time step of the simulator - copied from `step` input\n Scalar dt;\n\n /// \\brief The updated joint configuration of the system.\n VectorXs qnew;\n\n /// \\brief Free velocity i.e. the updated velocity of the system without any constraint forces.\n VectorXs vfree;\n\n /// \\brief The updated velocity, taking into account the correction due to constraint forces.\n VectorXs vnew;\n\n /// \\brief The updated acceleration, taking into account the correction due to constraint forces.\n VectorXs anew;\n\n /// \\brief Vector of total spatial forces (i.e. external forces + constraint forces) applied on\n /// joints, expressed in the local frame of each joint. Note: by subtracting the external forces\n /// (given to the `step` method of the simulator) to `ftotal`, we get the constraint forces\n /// expressed in the local frame of the joints.\n SpatialForceVector ftotal;\n\n /// \\brief Vector of total torques (i.e. applied tau + dry friction on joints + forces due to joint limits) applied on\n /// joints.\n VectorXs tau_total;\n\n /// \\brief Vector of constraint torques\n VectorXs tau_constraints;\n\n /// \\brief Base struct to set up constraint solvers\n struct ConstraintSolverSettings\n {\n int max_iter{1000};\n Scalar absolute_precision{1e-8};\n Scalar relative_precision{1e-8};\n bool stat_record{false};\n };\n\n /// \\brief Struct to store the settings for the ADMM constraint solver.\n ///\n /// Pinocchio's ADMM has two regularization term\n /// -> tau * rho for the augmented lagrangian term (consensus penalty)\n /// -> mu term for the proximal term (primal variable regularization)\n ///\n /// ADMM update rule: if ratio_primal_dual reached, rho *= rho_increment, where\n /// rho_increment = pow(L/m, rho_power_factor).\n ///\n /// Initialization of rho:\n /// -- If linear update rule -> rho is initialized by ADMM constructor or `setRho`\n /// -- If spectral update rule -> rho is initialized by ADMM `computeRho` = sqrt(L*m) * pow(L/m, rho_power)\n ///\n struct ADMMConstraintSolverSettings : ConstraintSolverSettings\n {\n Scalar mu{1e-6};\n Scalar tau{0.5};\n Scalar rho{10}; // initial value of rho for linear update rule\n Scalar rho_power{0.2}; // initial value of the rho_power for spectral update rule\n Scalar rho_power_factor{0.05};\n Scalar linear_update_rule_factor{2};\n Scalar ratio_primal_dual{50};\n int lanczos_size{3}; // higher leads to more accurate eigvalues estimation. Max size = constraint problem size\n ::pinocchio::ADMMUpdateRule admm_update_rule{::pinocchio::ADMMUpdateRule::SPECTRAL};\n };\n\n /// \\brief Settings for the ADMM constraint solver.\n ADMMConstraintSolverSettings admm_constraint_solver_settings;\n\n /// \\brief Struct to store the settings for the PGS constraint solver.\n struct PGSConstraintSolverSettings : ConstraintSolverSettings\n {\n Scalar over_relax{1.0};\n };\n\n /// \\brief Settings for the PGS constraint solver.\n PGSConstraintSolverSettings pgs_constraint_solver_settings;\n\n /// \\brief Type of constraint solver used in the last call to the `step` method.\n /// This enum is meant to indicate which solver was used, not which one to use.\n /// To use a specific constraint solver, use the template of the `step` method.\n enum struct ConstraintSolverType\n {\n PGS,\n ADMM,\n NONE\n };\n\n /// \\brief Type of constraint solver used in the last call to the `step` method.\n ConstraintSolverType constraint_solver_type;\n\n /// \\brief ADMM constraint solver.\n ADMMConstraintSolver admm_constraint_solver;\n\n /// \\brief PGS constraint solver.\n PGSConstraintSolver pgs_constraint_solver;\n\n /// \\brief Whether or not to warm-start the constraint solver.\n bool warm_start_constraints_forces{true};\n\n /// \\brief Whether or not timings of the `step` function are measured.\n /// If set to true, the timing of the last call to `step` can be accessed via `getCPUTimes`\n bool measure_timings{false};\n\n /// \\brief Get timings of the last call to the `step` method.\n hpp::fcl::CPUTimes getStepCPUTimes() const\n {\n return this->m_step_timings;\n }\n\n /// \\brief Get timings of the call to the constraint solver in the last call of `step`.\n /// This timing is set to 0 if there was no constraints.\n hpp::fcl::CPUTimes getConstraintSolverCPUTimes() const\n {\n return this->m_constraint_solver_timings;\n }\n\n /// \\brief Get timings of the collision detection stage.\n hpp::fcl::CPUTimes getCollisionDetectionCPUTimes() const\n {\n return this->m_collision_detection_timings;\n }\n\n /// \\brief Virtual destructor of the base class\n virtual ~SimulatorTpl()\n {\n }\n\n protected:\n /// \\brief Broad phase manager\n // TODO(louis): template by broad phase type (no broad phase, dynamic aabb tree, SAP etc)\n BroadPhaseManagerHandle m_broad_phase_manager;\n\n /// \\brief Collision callback\n CollisionCallbackCollect m_collision_callback;\n\n /// \\brief Constraint problem\n ConstraintsProblemHandle m_constraints_problem;\n\n /// \\brief Temporary variable used to integrate the system's state\n VectorXs m_vnew_integration_tmp;\n\n /// \\brief Whether or not the simulator has been reset.\n bool m_is_reset;\n\n /// \\brief Timer used for the whole `step` method\n hpp::fcl::Timer m_step_timer;\n\n /// \\brief Timings for the whole `step` method.\n hpp::fcl::CPUTimes m_step_timings;\n\n /// \\brief Timer used for internal methods inside `step`.\n hpp::fcl::Timer m_internal_timer;\n\n /// \\brief Timings for the call to the constraint solver.\n hpp::fcl::CPUTimes m_constraint_solver_timings;\n\n /// \\brief Timings for collision detection.\n hpp::fcl::CPUTimes m_collision_detection_timings;\n\n public:\n ///\n /// \\brief Constructor specifying the Data and GeometryData associated to the model and\n /// geom_model.\n SimulatorTpl(\n ModelHandle model_handle,\n DataHandle data_handle,\n GeometryModelHandle geom_model_handle,\n GeometryDataHandle geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models,\n const WeldConstraintModelVector & weld_constraint_models);\n\n ///\n /// \\brief Constructor using model, data, geometry model, geometry data and vector of bilateral constraints.\n SimulatorTpl(\n ModelHandle model_handle,\n DataHandle data_handle,\n GeometryModelHandle geom_model_handle,\n GeometryDataHandle geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models);\n\n ///\n /// \\brief Constructor specifying the Data and GeometryData associated to the model and\n /// geom_model.\n SimulatorTpl(\n ModelHandle model_handle,\n DataHandle data_handle,\n GeometryModelHandle geom_model_handle,\n GeometryDataHandle geom_data_handle,\n const WeldConstraintModelVector & weld_constraint_models);\n\n ///\n /// \\brief Constructor specifying the Data and GeometryData associated to the model and\n /// geom_model.\n SimulatorTpl(\n ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle);\n\n ///\n /// \\brief Default constructor\n /// Whenever the model or the geometry model is changed, this constructor should be called.\n SimulatorTpl(\n ModelHandle model_handle,\n GeometryModelHandle geom_model_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models,\n const WeldConstraintModelVector & weld_constraint_models);\n\n ///\n /// \\brief Default constructor\n /// Whenever the model or the geometry model is changed, this constructor should be called.\n SimulatorTpl(\n ModelHandle model_handle,\n GeometryModelHandle geom_model_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models);\n\n ///\n /// \\brief Default constructor\n /// Whenever the model or the geometry model is changed, this constructor should be called.\n SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle, const WeldConstraintModelVector & weld_constraint_models);\n\n ///\n /// \\brief Default constructor\n /// Whenever the model or the geometry model is changed, this constructor should be called.\n SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle);\n\n ///\n /// \\brief Resets the internal quantities of the simulator.\n /// This methods needs to be called before looping on the `step` method, for example when the\n /// initial state (q0, v0) of the system is used as an input to `step` and (q0, v0) have not\n /// been computed using the `step` method. \\note If, instead, the simulator's model, data,\n /// geom_model or geom_data have changed, please call the constructor.\n void reset();\n\n ///\n /// \\brief Main step function of the simulator.\n template<\n template class ConstraintSolver = ::pinocchio::ADMMContactSolverTpl,\n typename ConfigVectorType,\n typename VelocityVectorType,\n typename TorqueVectorType>\n void step(\n const Eigen::MatrixBase & q,\n const Eigen::MatrixBase & v,\n const Eigen::MatrixBase & tau,\n Scalar dt);\n\n ///\n /// \\brief Main step function of the simulator.\n /// fext must be expressed in the local frame of each joint.\n // TODO: remove aligned_vector and template by Allocator.\n // TODO: change MatrixBase to PlainObject\n template<\n template class ConstraintSolver = ::pinocchio::ADMMContactSolverTpl,\n typename ConfigVectorType,\n typename VelocityVectorType,\n typename TorqueVectorType,\n typename ForceDerived>\n void step(\n const Eigen::MatrixBase & q,\n const Eigen::MatrixBase & v,\n const Eigen::MatrixBase & tau,\n const pinocchio::container::aligned_vector & fext,\n const Scalar dt);\n\n ///\n /// \\brief Returns true if the simulator is in its reset state.\n bool isReset() const\n {\n return this->m_is_reset;\n }\n\n ///\n /// \\brief Check consistency of the simulator.\n bool check() const;\n\n ///\n /// \\brief Check consistency of the collision pairs.\n /// This method checks thath the geometry model does not contain collision pairs between\n /// geometry objects from the same parent joint.\n bool checkCollisionPairs() const;\n\n ///\n /// \\brief Returns a const reference to the model\n const Model & model() const\n {\n return pinocchio::helper::get_ref(this->m_model);\n }\n\n ///\n /// \\brief Returns a reference to the model\n Model & model()\n {\n return pinocchio::helper::get_ref(this->m_model);\n }\n\n ///\n /// \\brief Returns a const reference to the data\n const Data & data() const\n {\n return pinocchio::helper::get_ref(this->m_data);\n }\n\n ///\n /// \\brief Returns a reference to the data\n Data & data()\n {\n return pinocchio::helper::get_ref(this->m_data);\n }\n\n ///\n /// \\brief Returns a const reference to the geometry model\n const pinocchio::GeometryModel & geom_model() const\n {\n return pinocchio::helper::get_ref(this->m_geom_model);\n }\n\n ///\n /// \\brief Returns a reference to the geometry model\n pinocchio::GeometryModel & geom_model()\n {\n return pinocchio::helper::get_ref(this->m_geom_model);\n }\n\n ///\n /// \\brief Returns a const reference to the geometry data\n const pinocchio::GeometryData & geom_data() const\n {\n return pinocchio::helper::get_ref(this->m_geom_data);\n }\n\n ///\n /// \\brief Returns a reference to the geometry data\n pinocchio::GeometryData & geom_data()\n {\n return pinocchio::helper::get_ref(this->m_geom_data);\n }\n\n ///\n /// \\brief Returns a const reference to the constraint problem\n const ConstraintsProblem & constraints_problem() const\n {\n return pinocchio::helper::get_ref(this->m_constraints_problem);\n }\n\n ///\n /// \\brief Returns a const reference to the constraint problem\n ConstraintsProblem & constraints_problem()\n {\n return pinocchio::helper::get_ref(this->m_constraints_problem);\n }\n\n ///\n /// \\brief Returns a handle to the constraint problem\n ConstraintsProblemHandle getConstraintsProblemHandle() const\n {\n return this->m_constraints_problem;\n }\n\n protected:\n ///\n /// \\brief Allocates memory based on `model` and active collision pairs in `geom_model`.\n void allocate();\n\n ///\n /// \\brief Initializes the geometry data for broad and narrow phase collision detection.\n void initializeGeometryData();\n\n ///\n /// \\brief Warm starting constraint forces via constraint inverse dynamics.\n void warmStartConstraintForces();\n\n ///\n /// \\brief Collision detection\n void detectCollisions();\n\n ///\n /// \\brief Preambule function meant to be run before resolving collisions.\n virtual void preambleResolveConstraints(const Scalar dt)\n {\n PINOCCHIO_UNUSED_VARIABLE(dt);\n }\n\n ///\n /// \\brief Constraint resolution\n template class ConstraintSolver, typename VelocityVectorType>\n void resolveConstraints(const Eigen::MatrixBase & v, const Scalar dt);\n };\n\n namespace details\n {\n template class SolverTpl, typename _Scalar, int _Options, template class JointCollectionTpl>\n struct SimulatorConstraintSolverTpl\n {\n };\n\n template class JointCollectionTpl>\n struct SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl>\n {\n using Scalar = _Scalar;\n enum\n {\n Options = _Options\n };\n using Simulator = SimulatorTpl;\n using ConstraintsProblem = typename Simulator::ConstraintsProblem;\n using ADMMConstraintSolverSettings = typename Simulator::ADMMConstraintSolverSettings;\n using ADMMConstraintSolver = ::pinocchio::ADMMContactSolverTpl;\n\n using MatrixXs = typename Simulator::MatrixXs;\n using VectorXs = typename Simulator::VectorXs;\n using RefConstVectorXs = Eigen::Ref;\n\n static void run(Simulator & simulator, Scalar dt);\n\n protected:\n static void setup(Simulator & simulator);\n };\n\n template class JointCollectionTpl>\n struct SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl>\n {\n using Scalar = _Scalar;\n enum\n {\n Options = _Options\n };\n using Simulator = SimulatorTpl;\n using ConstraintsProblem = typename Simulator::ConstraintsProblem;\n using PGSConstraintSolverSettings = typename Simulator::PGSConstraintSolverSettings;\n using PGSConstraintSolver = ::pinocchio::PGSContactSolverTpl;\n using DelassusOperatorDense = ::pinocchio::DelassusOperatorDenseTpl;\n\n using MatrixXs = typename Simulator::MatrixXs;\n using VectorXs = typename Simulator::VectorXs;\n using RefConstVectorXs = Eigen::Ref;\n\n static void run(Simulator & simulator, Scalar dt);\n\n protected:\n static void setup(Simulator & simulator);\n };\n } // namespace details\n\n} // namespace simple\n\n#include \"simple/core/simulator.hxx\"\n\n#if SIMPLE_ENABLE_TEMPLATE_INSTANTIATION\n #include \"simple/core/simulator.txx\"\n #include \"simple/pinocchio_template_instantiation/aba.txx\"\n #include \"simple/pinocchio_template_instantiation/joint-model.txx\"\n #include \"simple/pinocchio_template_instantiation/crba.txx\"\n#endif\n\n#endif // ifndef __simple_core_simulator_hpp__\n" }, { "path": "include/simple/core/simulator.hxx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_simulator_hxx__\n#define __simple_core_simulator_hxx__\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\n#include \"simple/tracy.hpp\"\n\n// #include \n\n#include \n\n#include \n\n#include \n\nnamespace simple\n{\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle,\n DataHandle data_handle,\n GeometryModelHandle geom_model_handle,\n GeometryDataHandle geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models,\n const WeldConstraintModelVector & weld_constraint_models)\n : m_model(model_handle)\n , m_data(data_handle)\n , m_geom_model(geom_model_handle)\n , m_geom_data(geom_data_handle)\n , q(this->model().nq)\n , v(this->model().nv)\n , tau(this->model().nv)\n , fext(static_cast(this->model().njoints), Force::Zero())\n , dt(-1)\n , qnew(this->model().nq)\n , vfree(this->model().nv)\n , vnew(this->model().nv)\n , anew(this->model().nv)\n , ftotal(static_cast(this->model().njoints), Force::Zero())\n , tau_total(this->model().nv)\n , tau_constraints(this->model().nv)\n , constraint_solver_type(ConstraintSolverType::NONE)\n , admm_constraint_solver(0) // only create it when needed\n , pgs_constraint_solver(0) // only create it when needed\n , m_broad_phase_manager(\n new BroadPhaseManager(&helper::get_ref(model_handle), &helper::get_ref(geom_model_handle), &helper::get_ref(geom_data_handle)))\n , m_collision_callback(this->geom_model(), this->geom_data())\n , m_constraints_problem(new ConstraintsProblem(\n model_handle, data_handle, geom_model_handle, geom_data_handle, bilateral_constraint_models, weld_constraint_models))\n , m_vnew_integration_tmp(this->model().nv)\n , m_is_reset(true)\n , m_step_timer(false)\n , m_internal_timer(false)\n {\n this->initializeGeometryData();\n this->allocate();\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle,\n DataHandle data_handle,\n GeometryModelHandle geom_model_handle,\n GeometryDataHandle geom_data_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models)\n : SimulatorTpl(model_handle, data_handle, geom_model_handle, geom_data_handle, bilateral_constraint_models, WeldConstraintModelVector())\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle,\n DataHandle data_handle,\n GeometryModelHandle geom_model_handle,\n GeometryDataHandle geom_data_handle,\n const WeldConstraintModelVector & weld_constraint_models)\n : SimulatorTpl(\n model_handle, data_handle, geom_model_handle, geom_data_handle, BilateralPointConstraintModelVector(), weld_constraint_models)\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle)\n : SimulatorTpl(\n model_handle, data_handle, geom_model_handle, geom_data_handle, BilateralPointConstraintModelVector(), WeldConstraintModelVector())\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle,\n GeometryModelHandle geom_model_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models,\n const WeldConstraintModelVector & weld_constraint_models)\n : SimulatorTpl(\n model_handle,\n std::make_shared<::pinocchio::Data>(*model_handle),\n geom_model_handle,\n std::make_shared<::pinocchio::GeometryData>(*geom_model_handle),\n bilateral_constraint_models,\n weld_constraint_models)\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle,\n GeometryModelHandle geom_model_handle,\n const BilateralPointConstraintModelVector & bilateral_constraint_models)\n : SimulatorTpl(model_handle, geom_model_handle, bilateral_constraint_models, WeldConstraintModelVector())\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(\n ModelHandle model_handle, GeometryModelHandle geom_model_handle, const WeldConstraintModelVector & weld_constraint_models)\n : SimulatorTpl(model_handle, geom_model_handle, BilateralPointConstraintModelVector(), weld_constraint_models)\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n SimulatorTpl::SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle)\n : SimulatorTpl(model_handle, geom_model_handle, BilateralPointConstraintModelVector(), WeldConstraintModelVector())\n {\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorTpl::allocate()\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::allocate\");\n\n this->constraints_problem().allocate();\n //\n this->q.resize(this->model().nq);\n this->v.resize(this->model().nv);\n this->tau.resize(this->model().nv);\n this->fext.resize(static_cast(model().njoints), Force::Zero());\n //\n this->qnew.resize(this->model().nq);\n this->vfree.resize(this->model().nv);\n this->vnew.resize(this->model().nv);\n this->anew.resize(this->model().nv);\n this->anew.setZero();\n this->ftotal.resize(static_cast(model().njoints), Force::Zero());\n this->tau_total.resize(this->model().nv);\n this->tau_total.setZero();\n this->tau_constraints.resize(this->model().nv);\n this->tau_constraints.setZero();\n this->m_vnew_integration_tmp.resize(this->model().nv);\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorTpl::initializeGeometryData()\n {\n // Broad phase setup\n for (::pinocchio::GeometryObject & geom : this->geom_model().geometryObjects)\n {\n geom.geometry->computeLocalAABB();\n }\n\n // Narrow phase setup\n for (hpp::fcl::CollisionRequest & request : this->geom_data().collisionRequests)\n {\n request.enable_contact = true;\n }\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorTpl::reset()\n {\n // Reset constraint problem\n this->constraints_problem().clear();\n this->constraints_problem().constraints_forces().setZero();\n this->constraints_problem().previous_constraints_forces().setZero();\n\n // Reset constraint solver\n this->constraint_solver_type = ConstraintSolverType::NONE;\n\n // Reset timings\n this->m_step_timings.clear();\n this->m_constraint_solver_timings.clear();\n\n this->m_is_reset = true;\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n template class ConstraintSolver, typename ConfigVectorType, typename VelocityVectorType, typename TorqueVectorType>\n void SimulatorTpl::step(\n const Eigen::MatrixBase & q,\n const Eigen::MatrixBase & v,\n const Eigen::MatrixBase & tau,\n Scalar dt)\n {\n this->fext.assign((std::size_t)(model().njoints), Force::Zero());\n this->step(q, v, tau, this->fext, dt);\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n template<\n template\n class ConstraintSolver,\n typename ConfigVectorType,\n typename VelocityVectorType,\n typename TorqueVectorType,\n typename ForceDerived>\n void SimulatorTpl::step(\n const Eigen::MatrixBase & q_,\n const Eigen::MatrixBase & v_,\n const Eigen::MatrixBase & tau_,\n const ::pinocchio::container::aligned_vector & fext_,\n const Scalar dt_)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::step\");\n // clang-format off\n if (this->measure_timings) { this->m_step_timer.start(); };\n // clang-format on\n\n // TODO(louis): should we use check arguments or assert/throw?\n PINOCCHIO_CHECK_ARGUMENT_SIZE(\n this->vfree.size(), this->model().nv,\n \"The sizes of the free velocity of the simulator and the input velocity do not match. \"\n \"You problably changed your model, data, geom_model or geom_data and forgot to call \"\n \"allocate().\");\n PINOCCHIO_CHECK_ARGUMENT_SIZE(q_.size(), this->model().nq, \"The joint configuration vector is not of right size\");\n PINOCCHIO_CHECK_ARGUMENT_SIZE(v_.size(), this->model().nv, \"The joint velocity vector is not of right size\");\n PINOCCHIO_CHECK_ARGUMENT_SIZE(tau_.size(), this->model().nv, \"The joint torque vector is not of right size\");\n PINOCCHIO_CHECK_ARGUMENT_SIZE(\n fext_.size(), static_cast(this->model().njoints), \"The external forces vector is not of right size\");\n PINOCCHIO_CHECK_INPUT_ARGUMENT(dt_ >= 0, \"dt is not >= 0\");\n assert(this->check() && \"The simulator is not properly instanciated.\");\n\n // Record state of simulator\n this->q = q_;\n this->v = v_;\n this->tau = tau_;\n this->fext = fext_;\n this->dt = dt_;\n\n // Set up data for downstream algorithms\n this->data().q_in = q;\n this->data().v_in = v;\n this->data().tau_in = tau;\n\n // Compute the mass matrix of the system - used by the delassus operator\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::step - compute crba\");\n ::pinocchio::crba(this->model(), this->data(), q, pinocchio::Convention::WORLD);\n }\n\n // Compute free acceleration of the system\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::step - compute vfree (first aba)\");\n this->ftotal = fext;\n this->tau_total = tau;\n // clang-format off\n this->vfree = v + dt * ::pinocchio::aba(this->model(), this->data(), q, v, this->tau_total, this->ftotal, pinocchio::Convention::WORLD);\n // clang-format on\n }\n\n // Collision detection\n this->detectCollisions();\n\n // Update constraint problem with result of collision detection\n const bool compute_constraints_forces_warm_start = this->warm_start_constraints_forces && (!this->isReset());\n this->constraints_problem().update(compute_constraints_forces_warm_start);\n\n this->tau_constraints.setZero();\n if (this->constraints_problem().constraints_problem_size() > 0)\n {\n // Constraint resolution - compute the total joint forces (if there are any collisions) i.e. external forces + constraint forces\n this->resolveConstraints(v, dt);\n\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::step - compute vnew (second aba)\");\n this->anew = ::pinocchio::aba(this->model(), this->data(), q, v, this->tau_total, this->ftotal, pinocchio::Convention::WORLD);\n this->vnew = v + dt * this->anew;\n }\n }\n else\n {\n this->anew = (this->vfree - v) / dt; // TODO(quentin) do it differrently\n this->vnew = this->vfree;\n\n // Reset constraint solver timings\n this->m_constraint_solver_timings.clear();\n }\n\n this->m_vnew_integration_tmp = this->vnew * dt;\n ::pinocchio::integrate(this->model(), q, this->m_vnew_integration_tmp, this->qnew);\n this->m_is_reset = false;\n\n // clang-format off\n if (this->measure_timings) { this->m_step_timer.stop(); };\n this->m_step_timings = this->m_step_timer.elapsed();\n // clang-format on\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorTpl::detectCollisions()\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::detectCollisions\");\n\n // clang-format off\n if (this->measure_timings) { this->m_internal_timer.start(); }\n // clang-format on\n\n // Compute oMg for each geometry\n ::pinocchio::updateGeometryPlacements(this->model(), this->data(), this->geom_model(), this->geom_data());\n\n // Reset collision results - super important! Otherwise constraint from previous time step may be\n // detected between non-colliding geometries.\n for (hpp::fcl::CollisionResult & col_res : this->geom_data().collisionResults)\n {\n col_res.clear();\n }\n\n // Run broad + narrow phase collision detection\n const bool recompute_local_aabb = false; // already computed in `initializeGeometryData`\n this->m_broad_phase_manager->update(recompute_local_aabb);\n assert(this->m_broad_phase_manager->check() && \"The broad phase manager is not aligned with the geometry model.\");\n\n // --> Broad Phase\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::detectCollisions - Broad phase\");\n ::pinocchio::computeCollisions(*(this->m_broad_phase_manager), &this->m_collision_callback);\n }\n\n // --> Narrow Phase\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::detectCollisions - Narrow phase\");\n for (std::size_t i = 0; i < this->m_collision_callback.pair_indexes.size(); ++i)\n {\n const std::size_t pair_index = this->m_collision_callback.pair_indexes[i];\n const ::pinocchio::CollisionPair & cp = this->geom_model().collisionPairs[pair_index];\n const ::pinocchio::GeometryObject & obj1 = this->geom_model().geometryObjects[cp.first];\n const ::pinocchio::GeometryObject & obj2 = this->geom_model().geometryObjects[cp.second];\n\n // middle phase collision detection\n coal::CollisionRequest collision_request(this->geom_data().collisionRequests[pair_index]);\n bool obb_overlap = false;\n if (\n obj1.geometry->getNodeType() == coal::GEOM_PLANE || obj1.geometry->getNodeType() == coal::GEOM_HALFSPACE\n || obj2.geometry->getNodeType() == coal::GEOM_PLANE || obj2.geometry->getNodeType() == coal::GEOM_HALFSPACE)\n {\n obb_overlap = true;\n }\n else\n {\n coal::Transform3s oM1(toFclTransform3f(this->geom_data().oMg[cp.first]));\n coal::Transform3s oM2(toFclTransform3f(this->geom_data().oMg[cp.second]));\n const Scalar security_margin = collision_request.security_margin;\n //\n const coal::AABB aabb1 = obj1.geometry->aabb_local.expand(security_margin * 0.5);\n coal::OBB obb1;\n coal::convertBV(aabb1, oM1, obb1);\n //\n const coal::AABB aabb2 = obj1.geometry->aabb_local.expand(security_margin * 0.5);\n coal::OBB obb2;\n coal::convertBV(aabb2, oM2, obb2);\n obb_overlap = obb1.overlap(obb2);\n }\n try\n {\n if (obb_overlap)\n {\n pinocchio::computeCollision(this->geom_model(), this->geom_data(), pair_index, collision_request);\n ::pinocchio::computeContactPatch(this->geom_model(), this->geom_data(), pair_index);\n }\n }\n catch (std::logic_error & e)\n {\n PINOCCHIO_THROW_PRETTY(\n std::logic_error, \"Geometries with index go1: \" << cp.first << \" or go2: \" << cp.second\n << \" have produced an internal error within Coal.\\n what:\\n\"\n << e.what());\n }\n }\n }\n\n // clang-format off\n if (this->measure_timings) { this->m_internal_timer.start(); }\n this->m_collision_detection_timings = this->m_internal_timer.elapsed();\n // clang-format on\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n template class ConstraintSolver, typename VelocityVectorType>\n void SimulatorTpl::resolveConstraints(const Eigen::MatrixBase & v, const Scalar dt)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::resolveConstraints\");\n\n assert(\n !this->constraints_problem().constraint_models.empty() && \"Resolve collisions should not be called if there are no constraints.\");\n\n // Build the constraint quantities for the constraint solver\n // TODO(quentin): warm-start constraint forces via constraint inverse dynamics\n this->constraints_problem().build(this->vfree, v, v + dt * this->anew, dt);\n this->preambleResolveConstraints(dt);\n\n // Call the constraint solver\n // clang-format off\n if (this->measure_timings) { this->m_internal_timer.start(); };\n details::SimulatorConstraintSolverTpl::run(*this, dt);\n if (this->measure_timings) { this->m_internal_timer.stop(); };\n this->m_constraint_solver_timings = this->m_internal_timer.elapsed();\n // clang-format on\n\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"Simulator::resolveConstraints - apply constraint forces\");\n ::pinocchio::evalConstraintJacobianTransposeMatrixProduct(\n this->model(), this->data(), //\n this->constraints_problem().constraint_models, //\n this->constraints_problem().constraint_datas, //\n this->constraints_problem().constraints_forces(), //\n this->tau_constraints);\n this->tau_total += this->tau_constraints;\n }\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n bool SimulatorTpl::checkCollisionPairs() const\n {\n for (GeomIndex col_pair_id = 0; col_pair_id < this->geom_model().collisionPairs.size(); col_pair_id++)\n {\n const GeomIndex geom_id1 = this->geom_model().collisionPairs[col_pair_id].first;\n const GeomIndex geom_id2 = this->geom_model().collisionPairs[col_pair_id].second;\n const ::pinocchio::GeometryObject & geom1 = this->geom_model().geometryObjects[geom_id1];\n const ::pinocchio::GeometryObject & geom2 = this->geom_model().geometryObjects[geom_id2];\n if (geom1.parentJoint == geom2.parentJoint)\n {\n return false;\n }\n }\n return true;\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n bool SimulatorTpl::check() const\n {\n assert(this->m_model != nullptr && \"The model handle points to nullptr.\");\n assert(this->m_data != nullptr && \"The data handle points to nullptr.\");\n assert(this->m_geom_model != nullptr && \"The geometry model handle points to nullptr.\");\n assert(this->m_geom_data != nullptr && \"The geometry data handle points to nullptr.\");\n assert(this->m_broad_phase_manager->check() && \"The broad phase manager is not aligned with the geometry model.\");\n assert(this->vfree.size() == this->model().nv && \"The free velocity vector is not of right size.\");\n assert(this->vnew.size() == this->model().nv && \"The new velocity vector is not of right size.\");\n assert(this->ftotal.size() == static_cast(this->model().njoints) && \"The total force vector is not of right size.\");\n assert(this->constraints_problem().check() && \"The constraint problem is invalid.\");\n assert(\n this->checkCollisionPairs()\n && \"The GeometryModel contains collision pairs between GeometryObjects attached to the same \"\n \"joint.\");\n\n return static_cast(\n this->m_model && this->m_data //\n && this->m_geom_model //\n && this->m_geom_data //\n && this->m_broad_phase_manager->check() //\n && this->vfree.size() == this->model().nv //\n && this->vnew.size() == this->model().nv //\n && this->ftotal.size() == static_cast(model().njoints) //\n && this->constraints_problem().check() //\n && this->checkCollisionPairs());\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorTpl::warmStartConstraintForces()\n {\n boost::optional> lambda_guess(this->constraints_problem().constraints_forces());\n // ::pinocchio::computeContactForces(\n // this->model(), this->data(), this->constraints_problem().constraints_velocities_warmstarts(),\n // this->constraints_problem().constraint_models, this->constraints_problem().constraint_datas,\n // this->constraints_problem().contact_compliances(), this->contact_solver_info, lambda_guess);\n\n const auto problem_size = static_cast(this->constraints_problem().constraints_problem_size());\n this->constraints_problem().constraints_forces() = (this->data().lambda_c.head(problem_size));\n }\n\n namespace details\n {\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void\n SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::setup(Simulator & simulator)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"SimulatorConstraintSolver::setup\");\n ADMMConstraintSolver & solver = simulator.admm_constraint_solver;\n const ADMMConstraintSolverSettings & settings = simulator.admm_constraint_solver_settings;\n\n // TODO: should we reuse rho and rho_power or always reset them?\n const Scalar mu = settings.mu;\n const Scalar tau = settings.tau;\n const Scalar rho = settings.rho;\n const Scalar rho_power = settings.rho_power;\n const Scalar rho_power_factor = settings.rho_power_factor;\n const Scalar linear_update_rule_factor = settings.linear_update_rule_factor;\n const Scalar ratio_primal_dual = settings.ratio_primal_dual;\n const int lanczos_size = settings.lanczos_size;\n\n const auto problem_size = static_cast(simulator.constraints_problem().constraints_problem_size());\n if (\n simulator.isReset() || simulator.constraint_solver_type != Simulator::ConstraintSolverType::ADMM\n || solver.getPrimalSolution().size() != problem_size)\n {\n solver = ADMMConstraintSolver(\n problem_size, mu, tau, rho_power, rho_power_factor, //\n linear_update_rule_factor, ratio_primal_dual, lanczos_size);\n solver.setRho(rho);\n }\n else\n {\n solver.setProximalValue(mu);\n solver.setTau(tau);\n solver.setRho(rho);\n solver.setRhoPower(rho_power);\n solver.setRhoPowerFactor(rho_power_factor);\n solver.setLinearUpdateRuleFactor(linear_update_rule_factor);\n solver.setRatioPrimalDual(ratio_primal_dual);\n solver.setLanczosSize(lanczos_size);\n }\n simulator.constraint_solver_type = Simulator::ConstraintSolverType::ADMM;\n solver.setMaxIterations(settings.max_iter);\n solver.setAbsolutePrecision(settings.absolute_precision);\n solver.setRelativePrecision(settings.relative_precision);\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::run(\n Simulator & simulator, Scalar dt)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"SimulatorConstraintSolver::run\");\n ConstraintsProblem & constraints_problem = simulator.constraints_problem();\n ADMMConstraintSolver & solver = simulator.admm_constraint_solver;\n const ADMMConstraintSolverSettings & settings = simulator.admm_constraint_solver_settings;\n\n // Create/update constraint solver\n setup(simulator);\n PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();\n\n // Delassus\n typename Simulator::DelassusOperator G(constraints_problem.constraint_cholesky_decomposition);\n\n // Drift term\n auto g = constraints_problem.g();\n\n // Preconditioner\n auto preconditioner = constraints_problem.preconditioner();\n\n // Input/output of the solver\n auto constraints_forces = constraints_problem.constraints_forces();\n\n if (!simulator.warm_start_constraints_forces)\n {\n constraints_forces.setZero();\n }\n\n solver.solve(\n G, g, constraints_problem.constraint_models, dt, boost::make_optional((RefConstVectorXs)preconditioner), //\n boost::make_optional((RefConstVectorXs)constraints_forces), boost::none, constraints_problem.is_ncp, settings.admm_update_rule,\n settings.stat_record);\n\n // Get solution of the solver\n constraints_forces = solver.getPrimalSolution();\n // Get constraint velocities -> TODO: it's no velocities anymore\n constraints_problem.constraints_velocities() = solver.getDualSolution();\n if (simulator.constraints_problem().is_ncp)\n {\n constraints_problem.constraints_velocities() += -solver.getComplementarityShift();\n }\n\n // Get time scaling for derivatives\n ::pinocchio::internal::getTimeScalingFromAccelerationToConstraints(\n constraints_problem.constraint_models, dt, constraints_problem.time_scaling_acc_to_constraints());\n\n PINOCCHIO_EIGEN_MALLOC_ALLOWED();\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::setup(Simulator & simulator)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"SimulatorConstraintSolver::setup\");\n PGSConstraintSolver & solver = simulator.pgs_constraint_solver;\n const PGSConstraintSolverSettings & settings = simulator.pgs_constraint_solver_settings;\n\n const auto problem_size = static_cast(simulator.constraints_problem().constraints_problem_size());\n if (\n simulator.isReset() || simulator.constraint_solver_type != Simulator::ConstraintSolverType::PGS\n || solver.getPrimalSolution().size() != problem_size)\n {\n solver = PGSConstraintSolver(problem_size);\n }\n simulator.constraint_solver_type = Simulator::ConstraintSolverType::PGS;\n\n solver.setMaxIterations(settings.max_iter);\n solver.setAbsolutePrecision(settings.absolute_precision);\n solver.setRelativePrecision(settings.relative_precision);\n }\n\n // --------------------------------------------------------------------------\n template class JointCollectionTpl>\n void SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::run(\n Simulator & simulator, Scalar dt)\n {\n SIMPLE_TRACY_ZONE_SCOPED_N(\"SimulatorConstraintSolver::run\");\n ConstraintsProblem & constraints_problem = simulator.constraints_problem();\n PGSConstraintSolver & solver = simulator.pgs_constraint_solver;\n const PGSConstraintSolverSettings & settings = simulator.pgs_constraint_solver_settings;\n\n // Create constraint solver\n setup(simulator);\n\n // Delassus\n typename Simulator::DelassusOperator G(constraints_problem.constraint_cholesky_decomposition);\n const bool enforce_symmetry = true;\n const DelassusOperatorDense delassus_dense(G, enforce_symmetry);\n\n // Drift term\n auto g = constraints_problem.g();\n\n // Warm-start and solution of the constraint solver\n auto constraints_forces = constraints_problem.constraints_forces();\n\n if (!simulator.warm_start_constraints_forces)\n {\n constraints_forces.setZero();\n }\n\n solver.solve(\n delassus_dense, g, constraints_problem.constraint_models, dt, boost::make_optional((RefConstVectorXs)constraints_forces), //\n settings.over_relax, constraints_problem.is_ncp, settings.stat_record);\n\n // Get solution of the solver\n constraints_forces = solver.getPrimalSolution();\n // Get constraint velocities\n constraints_problem.constraints_velocities() = solver.getDualSolution();\n\n // Get time scaling for derivatives\n ::pinocchio::internal::getTimeScalingFromAccelerationToConstraints(\n constraints_problem.constraint_models, dt, constraints_problem.time_scaling_acc_to_constraints());\n }\n\n } // namespace details\n\n} // namespace simple\n\n#endif // __simple_core_simulator_hxx__\n" }, { "path": "include/simple/core/simulator.txx", "content": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#ifndef __simple_simulator_txx__\n#define __simple_simulator_txx__\n\n#include \"simple/core/simulator.hpp\"\n\nnamespace simple\n{\n extern template struct SimulatorTpl;\n\n extern template void\n SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n context::Scalar);\n\n extern template void\n SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &,\n context::Scalar);\n\n extern template void\n SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n context::Scalar);\n\n extern template void\n SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &,\n context::Scalar);\n\n extern template void SimulatorTpl::resolveConstraints<\n ::pinocchio::ADMMContactSolverTpl>(const Eigen::MatrixBase &, const Scalar);\n\n extern template void SimulatorTpl::resolveConstraints<\n ::pinocchio::PGSContactSolverTpl>(const Eigen::MatrixBase &, const Scalar);\n\n} // namespace simple\n\n#endif // __simple_simulator_txx__\n" }, { "path": "include/simple/fwd.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_fwd_hpp__\n#define __simple_fwd_hpp__\n\n#ifdef _WIN32\n #include \n #undef far\n #undef near\n#endif\n\n#include \n\n#ifdef SIMPLE_EIGEN_CHECK_MALLOC\n #ifndef EIGEN_RUNTIME_NO_MALLOC\n #define EIGEN_RUNTIME_NO_MALLOC_WAS_NOT_DEFINED\n #define EIGEN_RUNTIME_NO_MALLOC\n #endif\n#endif\n\n// #include \"simple/macros.hpp\"\n#include \"simple/deprecated.hpp\"\n#include \"simple/warning.hpp\"\n#include \"simple/config.hpp\"\n\n// Include Eigen components\n#include \n#include \n#include \n\n#ifdef SIMPLE_WITH_ACCELERATE_SUPPORT\n #include \n#endif\n\n#include \n\nnamespace simple\n{\n ///\n /// \\brief Common traits structure to fully define base classes for CRTP.\n ///\n template\n struct traits\n {\n };\n\n namespace context\n {\n typedef double Scalar;\n enum\n {\n Options = 0\n };\n\n // Common eigen types\n using Vector2s = Eigen::Matrix;\n using Vector3s = Eigen::Matrix;\n using Vector6s = Eigen::Matrix;\n using Matrix6s = Eigen::Matrix;\n using Matrix63s = Eigen::Matrix;\n using Matrix6Xs = Eigen::Matrix;\n using VectorXs = Eigen::Matrix;\n using MatrixXs = Eigen::Matrix;\n using RowMatrixXs = Eigen::Matrix;\n\n // Commong pinocchio types\n using Force = ::pinocchio::ForceTpl;\n using Motion = ::pinocchio::MotionTpl;\n using SE3 = ::pinocchio::SE3Tpl;\n using Model = ::pinocchio::ModelTpl;\n using Data = ::pinocchio::DataTpl;\n } // namespace context\n\n} // namespace simple\n\n#endif // ifndef __simple_fwd_hpp__\n" }, { "path": "include/simple/math/fwd.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_math_fwd_hpp__\n#define __simple_math_fwd_hpp__\n\n#include \"simple/fwd.hpp\"\n\nnamespace simple\n{\n} // namespace simple\n\n#endif // ifndef __simple_math_fwd_hpp__\n" }, { "path": "include/simple/math/qr.hpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_math_qr_hpp__\n#define __simple_math_qr_hpp__\n\n#include \"simple/math/fwd.hpp\"\n\n#include \n\nnamespace simple\n{\n namespace math\n {\n template\n struct SolveInPlaceWrapper : _SolverType\n {\n typedef _SolverType SolverType;\n\n template\n void solveInPlace(const Eigen::MatrixBase & mat) const\n {\n typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixType) res(mat);\n res = this->solve(mat);\n mat.const_cast_derived() = res;\n }\n\n }; // struct SolveInPlaceWrapper\n\n template\n struct SolveInPlaceWrapper> : Eigen::HouseholderQR<_MatrixType>\n {\n typedef Eigen::HouseholderQR<_MatrixType> SolverType;\n\n template\n void solveInPlace(const Eigen::MatrixBase & mat_) const\n {\n const Eigen::Index rank = (std::min)(this->rows(), this->cols());\n MatrixType & mat = mat_.const_cast_derived();\n\n mat.applyOnTheLeft(this->householderQ().setLength(rank).adjoint());\n\n this->m_qr.topLeftCorner(rank, rank).template triangularView().solveInPlace(mat.topRows(rank));\n mat.bottomRows(this->cols() - rank).setZero();\n }\n };\n\n } // namespace math\n} // namespace simple\n\n#endif // ifndef __simple_math_qr_hpp__\n" }, { "path": "include/simple/pch.hpp", "content": "// PINOCCHIO\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \n\n// EIGEN\n#include \n#include \n#include \n" }, { "path": "include/simple/pinocchio_template_instantiation/aba-derivatives.txx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_aba_derivatives_txx__\n#define __simple_pinocchio_template_instantiation_aba_derivatives_txx__\n\nextern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void ::pinocchio::computeABADerivatives<\n ::simple::context::Scalar,\n ::simple::context::Options,\n ::pinocchio::JointCollectionDefaultTpl,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::Force,\n Eigen::aligned_allocator<::simple::context::Force>,\n Eigen::Ref<::simple::context::RowMatrixXs>,\n Eigen::Ref<::simple::context::RowMatrixXs>,\n Eigen::Ref<::simple::context::RowMatrixXs>>(\n const ::simple::context::Model &,\n ::simple::context::Data &,\n const Eigen::MatrixBase<::simple::context::VectorXs> &,\n const Eigen::MatrixBase<::simple::context::VectorXs> &,\n const Eigen::MatrixBase<::simple::context::VectorXs> &,\n const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &,\n const Eigen::MatrixBase> &,\n const Eigen::MatrixBase> &,\n const Eigen::MatrixBase> &);\n\n#endif // __simple_pinocchio_template_instantiation_aba_derivatives_txx__\n" }, { "path": "include/simple/pinocchio_template_instantiation/aba.txx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_aba_txx__\n#define __simple_pinocchio_template_instantiation_aba_txx__\n\nextern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba<\n ::simple::context::Scalar,\n ::simple::context::Options,\n ::pinocchio::JointCollectionDefaultTpl,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs>(\n const ::simple::context::Model &,\n ::simple::context::Data &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Convention);\n\nextern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba<\n ::simple::context::Scalar,\n ::simple::context::Options,\n ::pinocchio::JointCollectionDefaultTpl,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::Force,\n Eigen::aligned_allocator<::simple::context::Force>>(\n const ::simple::context::Model &,\n ::simple::context::Data &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &,\n const Convention);\n\n#endif\n" }, { "path": "include/simple/pinocchio_template_instantiation/crba.txx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_crba_txx__\n#define __simple_pinocchio_template_instantiation_crba_txx__\n\nextern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::MatrixXs & ::pinocchio::\n crba<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs>(\n const context::Model &, context::Data &, const Eigen::MatrixBase &, const Convention convention);\n\n#endif // __simple_pinocchio_template_instantiation_crba_txx__\n" }, { "path": "include/simple/pinocchio_template_instantiation/joint-model.txx", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_joint_model_txx__\n#define __simple_pinocchio_template_instantiation_joint_model_txx__\n\nextern template struct ::pinocchio::\n JointModelTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>;\n\nextern template struct ::pinocchio::\n JointDataTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>;\n\n#endif\n" }, { "path": "include/simple/utils/visitors.hpp", "content": "//\n// Copyright (c) 2025 INRIA\n//\n\n#ifndef __simple_utils_visitors_hpp__\n#define __simple_utils_visitors_hpp__\n\n#include \n#include \n#include \n\nnamespace simple\n{\n namespace visitors\n {\n // Declaration of a helper to accumulate operator() overloads from lambdas.\n template\n struct lambda_visitor_helper;\n\n // Specialization for initial case without any lambda\n template\n struct lambda_visitor_helper\n {\n // Default constructor\n lambda_visitor_helper()\n {\n }\n\n // Operator that handles boost::blank case\n ReturnType operator()(const boost::blank &) const\n {\n PINOCCHIO_THROW_PRETTY(std::invalid_argument, \"Cannot call operator() on boost::blank.\");\n return ::pinocchio::visitors::internal::NoRun::run();\n }\n };\n\n // Specialization for recursion\n template\n struct lambda_visitor_helper\n : Lambda\n , lambda_visitor_helper\n {\n typedef lambda_visitor_helper RecursiveHelper;\n\n // Initialize lambda and the recursion\n lambda_visitor_helper(Lambda lambda, Lambdas... lambdas)\n : Lambda(lambda)\n , RecursiveHelper(lambdas...)\n {\n }\n\n // operator() is overloaded by lambda and all overloa of the recursion\n using Lambda::operator();\n using RecursiveHelper::operator();\n };\n\n // Wrapper struct around helper that inherit static_visitor\n // It is an additional class to avoid diamond inheritance\n template\n struct lambda_visitor\n : boost::static_visitor\n , lambda_visitor_helper\n {\n typedef lambda_visitor_helper Helper;\n\n lambda_visitor(Lambdas... lambdas)\n : Helper(lambdas...)\n {\n }\n\n using Helper::operator();\n };\n\n // Wrapper function to create a lambda_visitor instance.\n // This deduces the template arguments automatically, so you don't need to specify them.\n template\n lambda_visitor make_lambda_visitor(Lambdas... lambdas)\n {\n return lambda_visitor(lambdas...);\n }\n\n } // namespace visitors\n} // namespace simple\n\n#endif // __simple_utils_visitors_hpp__\n" }, { "path": "sandbox/cartpole.py", "content": "import pinocchio as pin\nimport numpy as np\nimport hppfcl\nimport tap\nfrom pinocchio.visualize import MeshcatVisualizer\nimport meshcat\nfrom simulation_args import SimulationArgs\nimport simple\n\nGREY = np.array([192, 201, 229, 255]) / 255\n\n\nclass SimulationArgs(tap.Tap):\n horizon: int = 1000\n dt: float = 1e-3\n contact_solver: str = \"ADMM\" # set to PGS or ADMM\n maxit: int = 1000 # solver maxit\n tol: float = 1e-6 # absolute constraint solver tol\n tol_rel: float = 1e-6 # relative constraint solver tol\n solve_ncp: int = 1 # set to 0 to solve ccp\n warm_start: int = 1 # warm start the solver?\n mu_prox: float = 1e-4 # prox value for admm\n material: str = \"metal\" # contact friction\n compliance: float = 0\n Kp: float = 0 # baumgarte proportional term\n Kd: float = 0 # baumgarte derivative term\n seed: int = 1234\n limits: bool = False # set to activate limits\n floor: bool = False # add a floor\n cos_torque: bool = False\n dont_stop: bool = False\n\n\nargs = SimulationArgs().parse_args()\nallowed_solvers = [\"ADMM\", \"PGS\"]\nif args.contact_solver not in allowed_solvers:\n print(\n f\"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting\"\n )\n exit(1)\nnp.random.seed(args.seed)\npin.seed(args.seed)\n\n\ndef create_cartpole(N, add_floor):\n model = pin.Model()\n geom_model = pin.GeometryModel()\n\n if add_floor:\n # add floor\n floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0)\n M = pin.SE3.Identity()\n floor_collision_object = pin.GeometryObject(\n \"floor\", 0, 0, M, floor_collision_shape\n )\n color = GREY\n color[3] = 0.5\n floor_collision_object.meshColor = color\n geom_floor = geom_model.addGeometryObject(floor_collision_object)\n\n parent_id = 0\n\n cart_radius = 0.1\n cart_length = 5 * cart_radius\n cart_mass = 1.0\n joint_name = \"joint_cart\"\n\n geometry_placement = pin.SE3.Identity()\n geometry_placement.rotation = pin.Quaternion(\n np.array([0.0, 0.0, 1.0]), np.array([0.0, 1.0, 0.0])\n ).toRotationMatrix()\n\n joint_id = model.addJoint(\n parent_id, pin.JointModelPY(), pin.SE3.Identity(), joint_name\n )\n\n body_inertia = pin.Inertia.FromCylinder(cart_mass, cart_radius, cart_length)\n body_placement = geometry_placement\n model.appendBodyToJoint(\n joint_id, body_inertia, body_placement\n ) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry\n\n shape_cart = hppfcl.Cylinder(cart_radius, cart_length)\n\n geom_cart = pin.GeometryObject(\n \"shape_cart\", joint_id, geometry_placement, shape_cart\n )\n geom_cart.meshColor = np.array([1.0, 0.1, 0.1, 1.0])\n geom_model.addGeometryObject(geom_cart)\n\n parent_id = joint_id\n joint_placement = pin.SE3.Identity()\n body_mass = 1.0\n body_radius = 0.1\n for k in range(N):\n joint_name = \"joint_\" + str(k + 1)\n joint_id = model.addJoint(\n parent_id, pin.JointModelRX(), joint_placement, joint_name\n )\n\n body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)\n body_placement = joint_placement.copy()\n body_placement.translation[2] = 1.0\n model.appendBodyToJoint(joint_id, body_inertia, body_placement)\n\n geom1_name = \"ball_\" + str(k + 1)\n shape1 = hppfcl.Sphere(body_radius)\n geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1)\n geom1_obj.meshColor = np.ones((4))\n geom_ball = geom_model.addGeometryObject(geom1_obj)\n if add_floor:\n geom_model.addCollisionPair(pin.CollisionPair(geom_floor, geom_ball))\n\n geom2_name = \"bar_\" + str(k + 1)\n shape2 = hppfcl.Cylinder(body_radius / 4.0, body_placement.translation[2])\n shape2_placement = body_placement.copy()\n shape2_placement.translation[2] /= 2.0\n\n geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2_placement, shape2)\n geom2_obj.meshColor = np.array([0.0, 0.0, 0.0, 1.0])\n geom_model.addGeometryObject(geom2_obj)\n\n # update parent id to add next pendulum\n parent_id = joint_id\n joint_placement = body_placement.copy()\n\n end_frame = pin.Frame(\n \"end_effector_frame\",\n model.getJointId(\"joint_\" + str(N)),\n 0,\n body_placement,\n pin.FrameType(3),\n )\n model.addFrame(end_frame)\n geom_model.collision_pairs = []\n model.qinit = np.zeros(model.nq)\n model.qinit[1] = 0.0 * np.pi\n model.qref = pin.neutral(model)\n return model, geom_model\n\n\n# ============================================================================\n# SCENE CREATION\n# ============================================================================\n# Create model\nmodel, geom_model = create_cartpole(1, args.floor)\n\nfor gobj in geom_model.geometryObjects:\n if args.material == \"ice\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE\n elif args.material == \"plastic\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC\n elif args.material == \"wood\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD\n elif args.material == \"metal\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL\n elif args.material == \"concrete\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE\n\n # Compliance\n gobj.physicsMaterial.compliance = args.compliance\n\n\n# Initial state\nif args.limits:\n for i in range(model.nq):\n model.lowerPositionLimit[i] = -0.8\n model.upperPositionLimit[i] = 0.8\nelse:\n for i in range(model.nq):\n model.lowerPositionLimit[i] = np.finfo(\"d\").min\n model.upperPositionLimit[i] = np.finfo(\"d\").max\nq0 = pin.neutral(model)\nv0 = np.zeros(model.nv)\ntau0 = np.ones(model.nv)\nprint(\"q0 = \", q0)\nprint(\"v0 = \", v0)\nprint(f\"{model.lowerPositionLimit}\")\nprint(f\"{model.upperPositionLimit}\")\n\n# visualize the trajectory\nviewer = meshcat.Visualizer(zmq_url=\"tcp://127.0.0.1:6000\")\nviewer.delete()\nvizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, geom_model)\nvizer.initViewer(viewer=viewer, open=False, loadModel=True)\nvizer.display(q0)\n\n# ============================================================================\n# SIMULATION\n# ============================================================================\nsim = simple.Simulator(model, geom_model)\n# PGS\nsim.pgs_constraint_solver_settings.absolute_precision = args.tol\nsim.pgs_constraint_solver_settings.relative_precision = args.tol_rel\nsim.pgs_constraint_solver_settings.max_iter = args.maxit\n# ADMM\nsim.admm_constraint_solver_settings.absolute_precision = args.tol\nsim.admm_constraint_solver_settings.relative_precision = args.tol_rel\nsim.admm_constraint_solver_settings.max_iter = args.maxit\nsim.admm_constraint_solver_settings.mu = args.mu_prox\n#\nsim.warm_start_contact_forces = args.warm_start\nsim.constraints_problem.is_ncp = args.solve_ncp\nsim.constraints_problem.Kp = args.Kp\nsim.constraints_problem.Kd = args.Kd\ndt = args.dt\nT = args.horizon\n\nq = q0.copy()\nv = v0.copy()\ntau = tau0.copy()\nzero_torque = np.zeros(model.nv)\nsim.step(q, v, tau0, dt)\nqprev = q.copy()\nvprev = v.copy()\nq = sim.qnew.copy()\nv = sim.qnew.copy()\nvizer.display(q)\ninput(\"[Press enter to simulate]\")\nfor t in range(T):\n tau = np.zeros(model.nv)\n if args.cos_torque:\n tau[0] = 10 * np.cos(10 * float(t) / float(T))\n if args.contact_solver == \"ADMM\":\n sim.step(q, v, tau, dt)\n if args.contact_solver == \"PGS\":\n sim.stepPGS(q, v, tau, dt)\n if sim.constraints_problem.constraints_problem_size() > 0 and not args.dont_stop:\n print(f\"{t=}\")\n print(f\"{qprev=}\")\n print(f\"{vprev=}\")\n print(f\"{q=}\")\n print(f\"{v=}\")\n print(f\"{sim.constraints_problem.constraints_problem_size()=}\")\n print(f\"{sim.constraints_problem.constraints_forces()=}\")\n input(\"[Press enter to continue]\")\n qprev = q.copy()\n vprev = v.copy()\n q = sim.qnew.copy()\n v = sim.vnew.copy()\n vizer.display(q)\n" }, { "path": "sandbox/cassie_mj.py", "content": "import pinocchio as pin\nimport numpy as np\nfrom simulation_args import SimulationArgs\nimport simple\nfrom robot_descriptions.loaders.pinocchio import load_robot_description as plr\nfrom sim_utils import (\n addMaterialAndCompliance,\n addFloor,\n addSystemCollisionPairs,\n SimulationArgs,\n createVisualizer,\n printSimulationPerfStats,\n setupSimulatorFromArgs,\n plotContactSolver,\n subSample,\n runMujocoXML,\n)\nimport time\n\nargs = SimulationArgs().parse_args()\nallowed_solvers = [\"ADMM\", \"PGS\"]\nif args.contact_solver not in allowed_solvers:\n print(\n f\"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting\"\n )\n exit(1)\nnp.random.seed(args.seed)\npin.seed(args.seed)\nmodel_path = \"cassie_mj_description\"\n\nif not args.mujoco:\n # Create model\n print(\"Loading mj robot description...\")\n robot_mj = plr(\"cassie_mj_description\")\n model = robot_mj.model\n geom_model = robot_mj.collision_model\n visual_model = robot_mj.visual_model\n constraint_models_dict = robot_mj.constraint_models\n\n for key, value in constraint_models_dict.items():\n print(\"\")\n print(f\"---Constraints of type {key}---\")\n for constraint in value:\n print(f\"{constraint.joint1_id=}\")\n print(f\"{constraint.joint1_placement.translation=}\")\n print(f\"{constraint.joint2_id=}\")\n print(f\"{constraint.joint2_placement.translation=}\")\n print(\"\")\n\n addMaterialAndCompliance(geom_model, args.material, args.compliance)\n\n print(f\"{model.damping=}\")\n\n # Initial state\n # q0 = model.referenceConfigurations[\"qpos0\"]\n q0 = model.referenceConfigurations[\"home\"]\n\n if args.random_init_vel:\n v0 = np.random.randn(model.nv)\n else:\n v0 = np.zeros(model.nv)\n # for i in range(model.nq):\n # model.lowerPositionLimit[i] = np.finfo(\"d\").min\n # model.upperPositionLimit[i] = np.finfo(\"d\").max\n # model.lowerDryFrictionLimit[:] = -0\n # model.upperDryFrictionLimit[:] = 0\n model.lowerDryFrictionLimit[:] = -1.0\n model.upperDryFrictionLimit[:] = 1.0\n model.lowerDryFrictionLimit[:6] = -0\n model.upperDryFrictionLimit[:6] = 0\n # for i in range(model.nq):\n # model.lowerPositionLimit[i] = -1.0\n # model.upperPositionLimit[i] = 1.0\n # print(f\"{model.lowerPositionLimit=}\")\n # print(f\"{model.upperPositionLimit=}\")\n\n # add floor and collision pairs\n addFloor(geom_model, visual_model)\n addSystemCollisionPairs(model, geom_model, q0)\n\n # visualize the trajectory\n vizer, _ = createVisualizer(model, geom_model, visual_model)\n vizer.display(q0)\n print(f\"{q0=}\")\n print(f\"{v0=}\")\n\n # simulation\n _data = model.createData()\n rmass = pin.computeTotalMass(model, _data)\n print(f\"Robot mass = {rmass}\")\n\n sim = simple.Simulator(model, geom_model, **constraint_models_dict)\n # sim = simple.Simulator(model, geom_model)\n setupSimulatorFromArgs(sim, args)\n dt = args.dt\n T = args.horizon\n\n input(\"[Press ENTER to simulate]\")\n q = q0.copy()\n qs = [q0.copy()]\n v = v0.copy()\n zero_torque = np.zeros(model.nv)\n step_timings = np.zeros(T)\n for t in range(T):\n start_time = time.time()\n if args.contact_solver == \"ADMM\":\n sim.step(q, v, zero_torque, dt)\n if args.contact_solver == \"PGS\":\n sim.stepPGS(q, v, zero_torque, dt)\n end_time = time.time()\n step_timings[t] = end_time - start_time\n\n plotContactSolver(sim, args, t, q, v)\n\n q = sim.qnew.copy()\n v = sim.vnew.copy()\n qs.append(q)\n if args.display or args.debug:\n vizer.display(q)\n\n printSimulationPerfStats(step_timings)\n\n fps = 60.0\n dt_vis = 1.0 / fps\n qs = subSample(qs, dt * T, fps)\n while True:\n for q in qs:\n step_start = time.time()\n vizer.display(q)\n time_until_next_step = dt_vis - (time.time() - step_start)\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\nelse:\n runMujocoXML(model_path, args)\n" }, { "path": "sandbox/force_action_derivative.py", "content": "import pinocchio as pin\nimport numpy as np\n\nnp.set_printoptions(suppress=True)\n\n# Testing the derivatives of M.act(f), where M is an SE3 element, f is a spatial force.\n# Derivative w.r.t M.\nfor i in range(1000):\n M: pin.SE3 = pin.SE3.Random()\n lam: pin.Force = pin.Force.Random()\n # res: pin.Force = M.act(lam)\n\n # Analytic jacobian\n P = np.zeros((6, 6))\n P[:3, 3:] = pin.skew(lam.linear)\n P[3:, :3] = pin.skew(lam.linear)\n P[3:, 3:] = pin.skew(lam.angular)\n J = -M.toDualActionMatrix() @ P\n\n # Finite diff jacobian\n Jfd = np.zeros((6, 6))\n eps = 1e-6\n ei = np.zeros(6)\n for i in range(6):\n ei[i] = eps\n M_plus = M * pin.exp6(ei)\n M_minus = M * pin.exp6(-ei)\n Jfd[:, i] = (M_plus.act(lam) - M_minus.act(lam)) / (2 * eps)\n ei[i] = 0\n\n assert np.linalg.norm(Jfd - J) <= 1e-8\n\nv = pin.Motion.Random()\nA = np.zeros((6, 6))\nA[:3, :3] = pin.skew(v.angular)\nA[:3, 3:] = pin.skew(v.linear)\nA[3:, 3:] = pin.skew(v.angular)\nprint(f\"Action = \\n{v.action}\")\nprint(f\"Action alamano = \\n{A}\")\n" }, { "path": "sandbox/four_bar_linkage.py", "content": "import pinocchio as pin\nimport numpy as np\nimport simple\nimport time\nfrom sim_utils import (\n addSystemCollisionPairs,\n SimulationArgs,\n createVisualizer,\n setupSimulatorFromArgs,\n plotContactSolver,\n subSample,\n runMujocoXML,\n printSimulationPerfStats,\n)\n\nargs = SimulationArgs().parse_args()\nmodel_path = \"./sandbox/robots/four_bar_linkage.xml\"\n\nif not args.mujoco:\n # create model\n model = pin.buildModelFromMJCF(model_path)\n bcms = pin.buildBilateralConstraintModelsFromMJCF(model, model_path)\n geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION)\n visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL)\n\n # initial state\n q0 = model.referenceConfigurations[\"qpos0\"]\n v0 = np.zeros(model.nv)\n print(f\"{q0=}\")\n print(f\"{v0=}\")\n\n addSystemCollisionPairs(model, geom_model, q0)\n # geom_model.removeAllCollisionPairs()\n\n for inertia in model.inertias:\n print(inertia)\n\n num = 0.4\n for i in range(model.nq):\n model.lowerPositionLimit[i] = -num\n model.upperPositionLimit[i] = num\n # for i in range(model.nq):\n # model.lowerPositionLimit[i] = np.finfo(\"d\").min\n # model.upperPositionLimit[i] = np.finfo(\"d\").max\n # model.lowerDryFrictionLimit[:] = -10000\n # model.upperDryFrictionLimit[:] = 10000\n model.lowerDryFrictionLimit[:] = 0\n model.upperDryFrictionLimit[:] = 0\n\n # visualize the trajectory\n vizer, _ = createVisualizer(model, geom_model, visual_model)\n vizer.display(q0)\n\n for inertia in model.inertias:\n print(inertia)\n print(list(bcms))\n print(bcms[0])\n sim = simple.Simulator(model, geom_model, bilateral_point_constraint_models=bcms)\n # sim = simple.Simulator(model, geom_model)\n setupSimulatorFromArgs(sim, args)\n dt = args.dt\n T = args.horizon\n\n input(\"[Press ENTER to simulate]\")\n q = q0.copy()\n qs = [q0.copy()]\n v = v0.copy()\n zero_torque = np.zeros(model.nv)\n step_timings = np.zeros(T)\n for t in range(T):\n start_time = time.time()\n if args.contact_solver == \"ADMM\":\n sim.step(q, v, zero_torque, dt)\n if args.contact_solver == \"PGS\":\n sim.stepPGS(q, v, zero_torque, dt)\n end_time = time.time()\n step_timings[t] = end_time - start_time\n\n plotContactSolver(sim, args, t, q, v)\n\n q = sim.qnew.copy()\n v = sim.vnew.copy()\n qs.append(q)\n if args.display or args.debug:\n vizer.display(q)\n\n printSimulationPerfStats(step_timings)\n\n fps = 60.0\n dt_vis = 1.0 / fps\n qs = subSample(qs, dt * T, fps)\n while True:\n for q in qs:\n step_start = time.time()\n vizer.display(q)\n time_until_next_step = dt_vis - (time.time() - step_start)\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\nelse:\n runMujocoXML(model_path, args)\n" }, { "path": "sandbox/four_five_bar_linkage.py", "content": "import pinocchio as pin\nimport numpy as np\nimport simple\nimport time\nfrom sim_utils import (\n addSystemCollisionPairs,\n SimulationArgs,\n createVisualizer,\n setupSimulatorFromArgs,\n plotContactSolver,\n subSample,\n runMujocoXML,\n printSimulationPerfStats,\n)\n\nargs = SimulationArgs().parse_args()\nmodel_path = \"./sandbox/robots/four_five_bar_linkage.xml\"\n\nif not args.mujoco:\n # create model\n model = pin.buildModelFromMJCF(model_path)\n wcms = pin.buildWeldConstraintModelsFromMJCF(model, model_path)\n geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION)\n visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL)\n\n # initial state\n q0 = model.referenceConfigurations[\"qpos0\"]\n v0 = np.zeros(model.nv)\n print(f\"{q0=}\")\n print(f\"{v0=}\")\n\n addSystemCollisionPairs(model, geom_model, q0)\n # geom_model.removeAllCollisionPairs()\n\n for inertia in model.inertias:\n print(inertia)\n\n num = 0.4\n for i in range(model.nq):\n model.lowerPositionLimit[i] = -num\n model.upperPositionLimit[i] = num\n # for i in range(model.nq):\n # model.lowerPositionLimit[i] = np.finfo(\"d\").min\n # model.upperPositionLimit[i] = np.finfo(\"d\").max\n # model.lowerDryFrictionLimit[:] = -10000\n # model.upperDryFrictionLimit[:] = 10000\n model.lowerDryFrictionLimit[:] = 0\n model.upperDryFrictionLimit[:] = 0\n\n # visualize the trajectory\n vizer, _ = createVisualizer(model, geom_model, visual_model)\n vizer.display(q0)\n\n for inertia in model.inertias:\n print(inertia)\n print(list(wcms))\n print(wcms[0])\n sim = simple.Simulator(model, geom_model, weld_constraint_models=[wcms[2]])\n # sim = simple.Simulator(model, geom_model)\n setupSimulatorFromArgs(sim, args)\n dt = args.dt\n T = args.horizon\n\n input(\"[Press ENTER to simulate]\")\n q = q0.copy()\n qs = [q0.copy()]\n v = v0.copy()\n zero_torque = np.zeros(model.nv)\n step_timings = np.zeros(T)\n for t in range(T):\n start_time = time.time()\n if args.contact_solver == \"ADMM\":\n sim.step(q, v, zero_torque, dt)\n if args.contact_solver == \"PGS\":\n sim.stepPGS(q, v, zero_torque, dt)\n end_time = time.time()\n step_timings[t] = end_time - start_time\n\n plotContactSolver(sim, args, t, q, v)\n\n q = sim.qnew.copy()\n v = sim.vnew.copy()\n qs.append(q)\n if args.display or args.debug:\n vizer.display(q)\n\n printSimulationPerfStats(step_timings)\n\n fps = 60.0\n dt_vis = 1.0 / fps\n qs = subSample(qs, dt * T, fps)\n while True:\n for q in qs:\n step_start = time.time()\n vizer.display(q)\n time_until_next_step = dt_vis - (time.time() - step_start)\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\nelse:\n runMujocoXML(model_path, args)\n" }, { "path": "sandbox/go2_contact_id.py", "content": "import pinocchio as pin\nimport numpy as np\nfrom pin_utils import addSystemCollisionPairs\nfrom simulation_utils import (\n addFloor,\n setPhysicsProperties,\n)\nfrom simulation_args import SimulationArgs\nimport simple\nfrom compute_derivatives import computeStepDerivatives, finiteDifferencesStep\nimport example_robot_data as erd\n\n\nclass ScriptArgs(SimulationArgs):\n go1: bool = False\n hand_stand: bool = False\n display_target_traj: bool = False\n noptim: int = 100\n step_size: float = 1e-3\n linesearch: bool = False\n debug: bool = False\n cpp: bool = False\n linesearch: bool = False\n save: bool = False\n maxit_linesearch: int = 1000\n min_step_size: float = 1e-14\n finite_differences: bool = False\n eps_fd: float = 1e-6\n\n\nargs = ScriptArgs().parse_args()\nallowed_solvers = [\"ADMM\", \"PGS\"]\nif args.contact_solver not in allowed_solvers:\n print(\n f\"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting\"\n )\n exit(1)\nnp.random.seed(args.seed)\npin.seed(args.seed)\n\n# ============================================================================\n# SCENE CREATION\n# ============================================================================\n# Create model\nif args.go1:\n robot = erd.load(\"go1\")\n model = robot.model\n geom_model = robot.collision_model\n visual_model = robot.visual_model\nelse:\n rmodel, rgeom_model, _ = pin.buildModelsFromMJCF(\"./robots/go2/mjcf/go2.xml\")\n ff_model = pin.Model()\n ff_id = ff_model.addJoint(\n 0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), \"robot_freeflyer\"\n )\n ff_model.addJointFrame(ff_id)\n ff_geom_model = pin.GeometryModel()\n frame_id = ff_model.getFrameId(\"robot_freeflyer\")\n model, geom_model = pin.appendModel(\n ff_model, rmodel, ff_geom_model, rgeom_model, frame_id, pin.SE3.Identity()\n )\n\n # Add plane in geom_model\n visual_model = geom_model.copy()\naddFloor(geom_model, visual_model)\nsetPhysicsProperties(geom_model, args.material, args.compliance)\n\n# Initial state\n# model.lowerPositionLimit = -np.ones((model.nq, 1))\n# model.upperPositionLimit = np.ones((model.nq, 1))\n# q0 = pin.randomConfiguration(model)\nif args.go1:\n q0 = model.referenceConfigurations[\"standing\"]\n if args.hand_stand:\n q0 = np.array(\n [\n 0.26,\n 0.0,\n 0.43,\n 0.0,\n 0.70710678,\n 0.0,\n 0.70710678,\n 0.0,\n -0.1,\n -1.6,\n 0.0,\n -0.1,\n -1.6,\n 0.0,\n 0.8,\n -1.853,\n 0.0,\n 0.8,\n -1.853,\n ]\n )\nelse:\n q0 = pin.neutral(model)\n q0[2] = 0.4\nv0 = np.zeros(model.nv)\nfext = [pin.Force(np.random.random(6)) for _ in range(model.njoints)]\nprint(\"q0 = \", q0)\nprint(\"v0 = \", v0)\naddSystemCollisionPairs(model, geom_model, q0)\n\n\nactuation = np.zeros((model.nv, model.nv - 6))\nactuation[6:, :] = np.eye(model.nv - 6)\n\ndata = model.createData()\ngeom_data = geom_model.createData()\nsimulator = simple.Simulator(model, data, geom_model, geom_data)\nsimulator.admm_constraint_solver_settings.absolute_precision = args.tol\nsimulator.admm_constraint_solver_settings.relative_precision = args.tol_rel\nsimulator.admm_constraint_solver_settings.max_iter = args.maxit\ndsim = simple.SimulatorDerivatives(simulator)\n\n\ndef computeCost(tau):\n simulator.reset()\n simulator.step(q0, v0, actuation @ tau, args.dt)\n acc = (simulator.vnew - v0) / args.dt\n cost = 0.5 * (np.linalg.norm(acc) ** 2)\n return cost\n\n\ntau_optim_init = np.zeros(actuation.shape[1])\n\nif args.save:\n costs = []\n grads = []\n\ntau_optim = tau_optim_init.copy()\nfor n in range(args.noptim):\n simulator.reset()\n simulator.step(q0, v0, actuation @ tau_optim, args.dt)\n if args.finite_differences:\n dvnew_dq, dvnew_dv, dvnew_dtau = finiteDifferencesStep(\n simulator, q0, v0, actuation @ tau_optim, args.dt\n )\n dvnew_dtau = dsim.dvnew_dtau.copy() @ actuation\n if args.cpp:\n dsim.stepDerivatives(simulator, q0, v0, actuation @ tau_optim, args.dt)\n dvnew_dtau = dsim.dvnew_dtau.copy() @ actuation\n else:\n dqnew_dq, dqnew_dv, dqnewdtau, dvnew_dq, dvnew_dv, dvnew_dtau = (\n computeStepDerivatives(\n simulator, q0, v0, actuation @ tau_optim, fext, args.dt\n )\n )\n dvnew_dtau = dvnew_dtau @ actuation\n if args.debug:\n print(f\"{dvnew_dtau=}\")\n print(f\"norm dvnew_dtau {np.linalg.norm(dvnew_dtau)}\")\n q = simulator.qnew.copy()\n v = simulator.vnew.copy()\n if args.debug:\n input()\n\n # Compute cost\n acc = (simulator.vnew - v0) / args.dt\n cost = 0.5 * np.dot(acc, acc)\n\n # Compute cost gradient\n grad_cost = dvnew_dtau.T @ acc\n\n if args.save:\n costs.append(cost)\n grads.append(np.linalg.norm(grad_cost))\n\n if args.linesearch:\n # Gauss newton step\n H_GN = np.dot(dvnew_dtau.T, dvnew_dtau)\n H_GN_inv = np.linalg.inv(H_GN + np.eye(model.nv - 6) * 1e-6)\n dtau = -H_GN_inv @ grad_cost\n expected_improvement = -0.5 * grad_cost @ dtau\n step_size = 1.0\n linesearch_it = 0\n while (\n step_size >= args.min_step_size and linesearch_it <= args.maxit_linesearch\n ):\n tau_optim_next = tau_optim + step_size * dtau\n cost_next = computeCost(tau_optim_next)\n if cost_next < cost - step_size * expected_improvement:\n break\n step_size /= 2\n linesearch_it += 1\n tau_optim = tau_optim_next\n else:\n # Gradient step\n tau_optim -= args.step_size * grad_cost\n\n print(f\"\\n---- ITERATION {n} ----\")\n print(f\"Current cost = {cost}\")\n print(f\"Current norm grad cost = {np.linalg.norm(grad_cost)}\")\n\nif args.save:\n costs = np.array(costs)\n grads = np.array(grads)\n if args.linesearch:\n method = \"GN\"\n else:\n method = \"GD\"\n if args.finite_differences:\n np.save(f\"./results/fd_{method}_costs_invdyn.npy\", costs)\n np.save(f\"./results/fd_{method}_grads_invdyn.npy\", grads)\n else:\n np.save(f\"./results/{method}_costs_invdyn.npy\", costs)\n np.save(f\"./results/{method}_grads_invdyn.npy\", grads)\n" }, { "path": "sandbox/humanoid_mj.py", "content": "import pinocchio as pin\nimport numpy as np\nimport simple\nfrom sim_utils import (\n # addFloor,\n addMaterialAndCompliance,\n addSystemCollisionPairs,\n plotContactSolver,\n runMujocoXML,\n setupSimulatorFromArgs,\n subSample,\n createVisualizer,\n SimulationArgs,\n printSimulationPerfStats,\n)\nimport time\n\nargs = SimulationArgs().parse_args()\nallowed_solvers = [\"ADMM\", \"PGS\"]\nif args.contact_solver not in allowed_solvers:\n print(\n f\"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting\"\n )\n exit(1)\nnp.random.seed(args.seed)\npin.seed(args.seed)\nmodel_path = \"./sandbox/robots/humanoid.xml\"\n\nif not args.mujoco:\n # Create model\n print(\"Loading mj robot description...\")\n model = pin.buildModelFromMJCF(model_path)\n geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION)\n visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL)\n\n addMaterialAndCompliance(geom_model, args.material, args.compliance)\n\n # Initial state\n q0 = model.referenceConfigurations[\"qpos0\"]\n\n for joint in model.joints:\n print(joint)\n\n if args.random_init_vel:\n v0 = np.random.randn(model.nv)\n else:\n v0 = np.zeros(model.nv)\n for i in range(model.nq):\n model.lowerPositionLimit[i] = np.finfo(\"d\").min\n model.upperPositionLimit[i] = np.finfo(\"d\").max\n # for i in range(model.nq):\n # model.positionLimitMargin[i] = np.finfo(\"d\").max\n model.lowerDryFrictionLimit[:] = 0\n model.upperDryFrictionLimit[:] = 0\n\n # add floor and collision pairs\n # addFloor(geom_model, visual_model)\n addSystemCollisionPairs(model, geom_model, q0)\n\n # visualize the trajectory\n vizer, _ = createVisualizer(model, geom_model, visual_model)\n vizer.display(q0)\n print(f\"{q0=}\")\n print(f\"{v0=}\")\n\n # simulation\n _data = model.createData()\n rmass = pin.computeTotalMass(model, _data)\n print(f\"Robot mass = {rmass}\")\n\n sim = simple.Simulator(model, geom_model)\n setupSimulatorFromArgs(sim, args)\n dt = args.dt\n T = args.horizon\n\n N = 1\n input(\"[Press ENTER to simulate]\")\n for i in range(N):\n q = q0.copy()\n qs = [q0.copy()]\n v = v0.copy()\n zero_torque = np.zeros(model.nv)\n step_timings = np.zeros(T)\n sim.reset()\n for t in range(T):\n start_time = time.time()\n if args.contact_solver == \"ADMM\":\n sim.step(q, v, zero_torque, dt)\n if args.contact_solver == \"PGS\":\n sim.stepPGS(q, v, zero_torque, dt)\n end_time = time.time()\n step_timings[t] = end_time - start_time\n\n plotContactSolver(sim, args, t, q, v)\n\n q = sim.qnew.copy()\n v = sim.vnew.copy()\n qs.append(q)\n if args.display and args.debug:\n vizer.display(q)\n\n printSimulationPerfStats(step_timings)\n\n if args.display:\n fps = 60.0\n dt_vis = 1.0 / fps\n qs = subSample(qs, dt * T, fps)\n while True:\n for q in qs:\n step_start = time.time()\n vizer.display(q)\n time_until_next_step = dt_vis - (time.time() - step_start)\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\nelse:\n runMujocoXML(model_path, args)\n" }, { "path": "sandbox/parallel_rollout.py", "content": "import pinocchio as pin\nimport numpy as np\nimport concurrent.futures\nimport timeit\nimport simple\nimport os\nimport time\n\nfrom simulation_utils import (\n addFloor,\n setPhysicsProperties,\n)\nfrom pin_utils import addSystemCollisionPairs\nfrom pinocchio.visualize import MeshcatVisualizer\n\ncurrent_dir = os.path.dirname(os.path.abspath(__file__))\n\n\ndef createSimulator(\n model: pin.Model,\n geom_model: pin.GeometryModel,\n max_num_contacts: int = 4,\n tol: float = 1e-8,\n tol_rel: float = 1e-12,\n mu_prox: float = 1e-4,\n maxit: int = 1000,\n Kp: float = 0.0,\n Kd: float = 0.0,\n):\n data = model.createData()\n geom_data = geom_model.createData()\n simulator = simple.Simulator(model, data, geom_model, geom_data)\n simulator.admm_constraint_solver_settings.absolute_precision = tol\n simulator.admm_constraint_solver_settings.relative_precision = tol_rel\n simulator.admm_constraint_solver_settings.max_iter = maxit\n simulator.admm_constraint_solver_settings.mu = mu_prox\n simulator.constraints_problem.setMaxNumberOfContactsPerCollisionPair(\n max_num_contacts\n )\n simulator.constraints_problem.Kp = Kp\n simulator.constraints_problem.Kd = Kd\n return simulator\n\n\nmodel = pin.buildModelFromMJCF(f\"{current_dir}/robots/go2/mjcf/go2.xml\")\ngeom_model = pin.buildGeomFromMJCF(\n model, f\"{current_dir}/robots/go2/mjcf/go2.xml\", pin.COLLISION\n)\n\nmaterial = \"concrete\"\ncompliance = 0.0\n\nvisual_model = geom_model.copy()\naddFloor(geom_model, visual_model)\nsetPhysicsProperties(geom_model, material, compliance)\n\nq = pin.neutral(model)\nq[2] = 0.5\nv = np.zeros(model.nv)\naddSystemCollisionPairs(model, geom_model, q)\n\nbatch_size = 1000\ntime_steps = 200\ndt = 2e-3\n# max_workers = 8\nmax_workers = os.cpu_count() # from: https://docs.python.org/3/library/concurrent.futures.html#concurrent.futures.ThreadPoolExecutor\n\ntau_batch = np.random.randn(batch_size, time_steps, model.nv) * 1.0\nsim_batch = [createSimulator(model, geom_model) for _ in range(batch_size)]\n\n# vizer = MeshcatVisualizer(model, geom_model, visual_model)\n# vizer.initViewer(loadModel=True, open=True)\n# vizer.display(q)\n\n\ndef simulate_tau_batch_sequential(tau_batch, sim_batch, q, v, dt):\n result_list = []\n for tau_traj, sim in zip(tau_batch, sim_batch):\n sim.reset()\n q_ = q.copy()\n v_ = v.copy()\n for tau in tau_traj:\n sim.step(q_, v_, tau, dt)\n q_ = sim.qnew\n v_ = sim.vnew\n # vizer.display(q_)\n # time.sleep(0.1)\n result_list.append((q_, v_))\n\n return result_list\n\n\ndef simulate_tau_batch_parallel(sim_batch, q, v, tau_batch, dt):\n def rollout_single(sim, tau):\n q_ = q.copy()\n v_ = v.copy()\n sim.rollout(q_, v_, tau, dt) # rollout without GIL\n return sim.qnew, sim.vnew\n\n with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as executor:\n futures = [\n executor.submit(rollout_single, sim, tau)\n for sim, tau in zip(sim_batch, tau_batch)\n ]\n results = [f.result() for f in futures]\n\n return results\n\n\n# check if results match\nresults_test_sequential = simulate_tau_batch_sequential(\n tau_batch[:3], sim_batch[:3], q, v, dt\n)\nresults_test_parallel = simulate_tau_batch_parallel(\n sim_batch[:3], q, v, tau_batch[:3, :3], dt\n)\n\nfor i in range(3):\n assert np.allclose(results_test_sequential[i][0], results_test_parallel[i][0])\n assert np.allclose(results_test_sequential[i][1], results_test_parallel[i][1])\n\nprint(\n f\"Running {batch_size} rollouts with {time_steps} time steps each and dt = {dt} seconds. Overall trajectory time = {time_steps * dt} seconds.\"\n)\nprint(f\"Number of workers: {max_workers}\")\n\nexecution_time_sequential = timeit.timeit(\n stmt=\"simulate_tau_batch_sequential(tau_batch, sim_batch, q, v, dt)\",\n setup=\"from __main__ import simulate_tau_batch_sequential, tau_batch, sim_batch, q, v, dt\",\n number=10,\n)\n\nprint(f\"Execution time [sequential]: {execution_time_sequential:.6f} seconds\")\n\nexecution_time_parallel = timeit.timeit(\n stmt=\"simulate_tau_batch_parallel(sim_batch, q, v, tau_batch, dt)\",\n setup=\"from __main__ import simulate_tau_batch_parallel, sim_batch, q, v, tau_batch, dt\",\n number=10,\n)\n\nprint(f\"Execution time [parallel]: {execution_time_parallel:.6f} seconds\")\nprint(f\"Speedup: {execution_time_sequential / execution_time_parallel:.2f}x\")\n" }, { "path": "sandbox/pendulum.py", "content": "import pinocchio as pin\nimport numpy as np\nimport simple\nimport time\nfrom sim_utils import (\n addSystemCollisionPairs,\n SimulationArgs,\n createVisualizer,\n setupSimulatorFromArgs,\n plotContactSolver,\n subSample,\n runMujocoXML,\n printSimulationPerfStats,\n)\n\nargs = SimulationArgs().parse_args()\nmodel_path = \"./sandbox/robots/pendulum.xml\"\n\nif not args.mujoco:\n model = pin.buildModelFromMJCF(model_path)\n constraint_models = pin.buildConstraintModelsFromMJCF(model, model_path)\n geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION)\n visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL)\n\n q0 = model.referenceConfigurations[\"qpos0\"]\n q0[0] = 0.4\n v0 = np.zeros(model.nv)\n print(f\"{q0=}\")\n print(f\"{v0=}\")\n\n addSystemCollisionPairs(model, geom_model, q0)\n # geom_model.removeAllCollisionPairs()\n\n for inertia in model.inertias:\n print(inertia)\n\n for i in range(model.nq):\n model.lowerPositionLimit[i] = -0.2\n model.upperPositionLimit[i] = 0.6\n # for i in range(model.nq):\n # model.lowerPositionLimit[i] = np.finfo(\"d\").min\n # model.upperPositionLimit[i] = np.finfo(\"d\").max\n # model.lowerDryFrictionLimit[:] = -10000\n # model.upperDryFrictionLimit[:] = 10000\n\n # visualize the trajectory\n vizer, _ = createVisualizer(model, geom_model, visual_model)\n vizer.display(q0)\n\n # sim = simple.Simulator(model, geom_model, constraint_models)\n sim = simple.Simulator(model, geom_model)\n setupSimulatorFromArgs(sim, args)\n dt = args.dt\n T = args.horizon\n\n input(\"[Press ENTER to simulate]\")\n q = q0.copy()\n qs = [q0.copy()]\n v = v0.copy()\n zero_torque = np.zeros(model.nv)\n step_timings = np.zeros(T)\n for t in range(T):\n start_time = time.time()\n if args.contact_solver == \"ADMM\":\n sim.step(q, v, zero_torque, dt)\n if args.contact_solver == \"PGS\":\n sim.stepPGS(q, v, zero_torque, dt)\n end_time = time.time()\n step_timings[t] = end_time - start_time\n\n plotContactSolver(sim, args, t, q, v)\n\n q = sim.qnew.copy()\n v = sim.vnew.copy()\n qs.append(q)\n if args.display or args.debug:\n vizer.display(q)\n\n printSimulationPerfStats(step_timings)\n\n fps = 60.0\n dt_vis = 1.0 / fps\n qs = subSample(qs, dt * T, fps)\n while True:\n for q in qs:\n step_start = time.time()\n vizer.display(q)\n time_until_next_step = dt_vis - (time.time() - step_start)\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\nelse:\n runMujocoXML(model_path, args)\n" }, { "path": "sandbox/pin_utils.py", "content": "import hppfcl\nimport pinocchio as pin\n\n\ndef addSystemCollisionPairs(model, geom_model, qref):\n \"\"\"\n Add the right collision pairs of a model, given qref.\n qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision\n in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered\n to always be in collision.\n \"\"\"\n data = model.createData()\n geom_data = geom_model.createData()\n pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref)\n geom_model.removeAllCollisionPairs()\n num_col_pairs = 0\n for i in range(len(geom_model.geometryObjects)):\n for j in range(i + 1, len(geom_model.geometryObjects)):\n # Don't add collision pair if same object\n if i != j:\n gobj_i: pin.GeometryObject = geom_model.geometryObjects[i]\n gobj_j: pin.GeometryObject = geom_model.geometryObjects[j]\n if gobj_i.name == \"floor\" or gobj_j.name == \"floor\":\n num_col_pairs += 1\n col_pair = pin.CollisionPair(i, j)\n geom_model.addCollisionPair(col_pair)\n else:\n if (\n gobj_i.parentJoint != gobj_j.parentJoint\n or gobj_i.parentJoint == 0\n ):\n if (\n gobj_i.parentJoint != model.parents[gobj_j.parentJoint]\n and gobj_j.parentJoint != model.parents[gobj_i.parentJoint]\n or gobj_i.parentJoint == 0\n or gobj_j.parentJoint == 0\n ):\n # Compute collision between the geometries. Only add the collision pair if there is no collision.\n M1 = geom_data.oMg[i]\n M2 = geom_data.oMg[j]\n colreq = hppfcl.CollisionRequest()\n colreq.security_margin = 1e-2 # 1cm of clearance\n colres = hppfcl.CollisionResult()\n hppfcl.collide(\n gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres\n )\n if not colres.isCollision():\n num_col_pairs += 1\n col_pair = pin.CollisionPair(i, j)\n geom_model.addCollisionPair(col_pair)\n print(\"Num col pairs = \", num_col_pairs)\n" }, { "path": "sandbox/robots/four_bar_linkage.xml", "content": "\n \n \n" }, { "path": "sandbox/robots/four_five_bar_linkage.xml", "content": "\n \n \n" }, { "path": "sandbox/robots/go2/mjcf/go2.xml", "content": "\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "sandbox/robots/go2/mjcf/scene.xml", "content": "\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n" }, { "path": "sandbox/robots/humanoid.xml", "content": "\n\n\n \n\n" }, { "path": "sandbox/robots/pendulum.xml", "content": "\n \n \n" }, { "path": "sandbox/sim_utils.py", "content": "import pinocchio as pin\nimport hppfcl\nimport numpy as np\nimport tap\nimport simple\nimport matplotlib.pyplot as plt\nimport time\nimport meshcat\nfrom pinocchio.visualize import MeshcatVisualizer\n\ntry:\n import mujoco\n from robot_descriptions.loaders.mujoco import load_robot_description as mlr\n\n mujoco_imported = True\nexcept:\n mujoco_imported = False\n\n\nclass SimulationArgs(tap.Tap):\n horizon: int = 1000\n dt: float = 1e-3\n contact_solver: str = \"ADMM\" # set to PGS or ADMM\n maxit: int = 100 # solver maxit\n tol: float = 1e-6 # absolute constraint solver tol\n tol_rel: float = 1e-6 # relative constraint solver tol\n warm_start: int = 1 # warm start the solver?\n mu_prox: float = 1e-4 # prox value for admm\n material: str = \"metal\" # contact friction\n compliance: float = 0\n max_contacts_per_pair: int = 4\n mujoco: bool = False\n Kp: float = 0 # baumgarte proportional term\n Kd: float = 0 # baumgarte derivative term\n seed: int = 1234\n random_init_vel: bool = False\n display: bool = False\n display_traj: bool = False\n solve_ccp: bool = False\n debug: bool = False\n debug_step: int = -1\n dont_stop: bool = False\n admm_update_rule: str = \"spectral\"\n ratio_primal_dual: float = 10\n tau: float = 0.5\n rho: float = 10.0\n rho_power: float = 0.05\n rho_power_factor: float = 0.05\n lanczos_size: int = 3\n linear_update_rule_factor: float = 2\n\n\ndef setupSimulatorFromArgs(sim: simple.Simulator, args: SimulationArgs):\n sim.warm_start_contact_forces = args.warm_start\n sim.constraints_problem.setMaxNumberOfContactsPerCollisionPair(\n args.max_contacts_per_pair\n )\n sim.constraints_problem.Kp = args.Kp\n sim.constraints_problem.Kd = args.Kd\n sim.constraints_problem.is_ncp = not args.solve_ccp\n # PGS\n sim.pgs_constraint_solver_settings.stat_record = args.debug\n sim.pgs_constraint_solver_settings.max_iter = args.maxit\n sim.pgs_constraint_solver_settings.absolute_precision = args.tol\n sim.pgs_constraint_solver_settings.relative_precision = args.tol_rel\n # ADMM\n sim.admm_constraint_solver_settings.stat_record = args.debug\n sim.admm_constraint_solver_settings.max_iter = args.maxit\n sim.admm_constraint_solver_settings.absolute_precision = args.tol\n sim.admm_constraint_solver_settings.relative_precision = args.tol_rel\n sim.admm_constraint_solver_settings.ratio_primal_dual = args.ratio_primal_dual\n sim.admm_constraint_solver_settings.mu = args.mu_prox\n sim.admm_constraint_solver_settings.rho = args.rho\n sim.admm_constraint_solver_settings.tau = args.tau\n if args.admm_update_rule == \"spectral\":\n sim.admm_constraint_solver_settings.admm_update_rule = (\n pin.ADMMUpdateRule.SPECTRAL\n )\n sim.admm_constraint_solver_settings.rho_power_factor = args.rho_power_factor\n sim.admm_constraint_solver_settings.rho_power = args.rho_power\n sim.admm_constraint_solver_settings.lanczos_size = args.lanczos_size\n elif args.admm_update_rule == \"linear\":\n sim.admm_constraint_solver_settings.admm_update_rule = pin.ADMMUpdateRule.LINEAR\n sim.admm_constraint_solver_settings.linear_update_rule_factor = (\n args.linear_update_rule_factor\n )\n elif args.admm_update_rule == \"constant\":\n sim.admm_constraint_solver_settings.admm_update_rule = (\n pin.ADMMUpdateRule.CONSTANT\n )\n\n\ndef plotContactSolver(\n sim: simple.Simulator,\n args: SimulationArgs,\n t: int,\n q: np.ndarray,\n v: np.ndarray,\n):\n if args.debug or t == args.debug_step:\n stats: pin.SolverStats = sim.admm_constraint_solver.getStats()\n if args.contact_solver == \"ADMM\":\n solver = sim.admm_constraint_solver\n if args.contact_solver == \"PGS\":\n solver = sim.pgs_constraint_solver\n stats = solver.getStats()\n abs_res = solver.getAbsoluteConvergenceResidual()\n rel_res = solver.getRelativeConvergenceResidual()\n it = solver.getIterationCount()\n if stats.size() > 0:\n plt.cla()\n title = (\n f\"Step {t}, it = {it}, abs res = {abs_res:.2e}, rel res = {rel_res:.2e}\"\n )\n if args.contact_solver == \"ADMM\":\n title += f\", cholesky count: {stats.cholesky_update_count}\"\n plt.title(title)\n plt.plot(stats.primal_feasibility, label=\"primal feas\")\n plt.plot(stats.dual_feasibility, label=\"dual feas\")\n plt.plot(stats.dual_feasibility_ncp, label=\"dual feas NCP\")\n if args.contact_solver == \"ADMM\":\n plt.plot(stats.dual_feasibility_admm, label=\"dual feas ADMM\")\n plt.plot(\n stats.dual_feasibility_constraint, label=\"dual feas constraint\"\n )\n plt.plot(stats.rho, label=\"rho\")\n if args.contact_solver == \"PGS\":\n plt.plot(stats.complementarity, label=\"complementarity\")\n plt.yscale(\"log\")\n plt.legend()\n plt.ion()\n plt.show()\n print(f\"{t=}\")\n print(f\"{q=}\")\n print(f\"{v=}\")\n print(f\"{sim.qnew=}\")\n print(f\"{sim.vnew=}\")\n print(f\"{sim.admm_constraint_solver.getIterationCount()=}\")\n print(f\"{sim.pgs_constraint_solver.getIterationCount()=}\")\n print(f\"{sim.constraints_problem.constraints_forces=}\")\n print(f\"{sim.constraints_problem.constraints_problem_size=}\")\n print(f\"{sim.constraints_problem.joint_friction_constraint_size=}\")\n print(f\"{sim.constraints_problem.joint_limit_constraint_size=}\")\n print(f\"{sim.constraints_problem.bilateral_constraints_size=}\")\n print(f\"{sim.constraints_problem.weld_constraints_size=}\")\n print(f\"{sim.constraints_problem.frictional_point_constraints_size=}\")\n print(\"Constraint solver timings: \", sim.getConstraintSolverCPUTimes().user)\n input(\"[Press enter to continue]\")\n\n\ndef subSample(xs, duration, fps):\n nb_frames = len(xs)\n nb_subframes = int(duration * fps)\n if nb_frames < nb_subframes:\n return xs\n else:\n step = nb_frames // nb_subframes\n xs_sub = [xs[i] for i in range(0, nb_frames, step)]\n return xs_sub\n\n\ndef addSystemCollisionPairs(model, geom_model, qref):\n \"\"\"\n Add the right collision pairs of a model, given qref.\n qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision\n in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered\n to always be in collision.\n \"\"\"\n data = model.createData()\n geom_data = geom_model.createData()\n pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref)\n geom_model.removeAllCollisionPairs()\n num_col_pairs = 0\n for i in range(len(geom_model.geometryObjects)):\n for j in range(i + 1, len(geom_model.geometryObjects)):\n # Don't add collision pair if same object\n if i != j:\n gobj_i: pin.GeometryObject = geom_model.geometryObjects[i]\n gobj_j: pin.GeometryObject = geom_model.geometryObjects[j]\n if gobj_i.name == \"floor\" or gobj_j.name == \"floor\":\n num_col_pairs += 1\n col_pair = pin.CollisionPair(i, j)\n geom_model.addCollisionPair(col_pair)\n else:\n if gobj_i.parentJoint != gobj_j.parentJoint:\n # Compute collision between the geometries. Only add the collision pair if there is no collision.\n M1 = geom_data.oMg[i]\n M2 = geom_data.oMg[j]\n colreq = hppfcl.CollisionRequest()\n colreq.security_margin = 1e-2 # 1cm of clearance\n colres = hppfcl.CollisionResult()\n hppfcl.collide(\n gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres\n )\n if not colres.isCollision():\n num_col_pairs += 1\n col_pair = pin.CollisionPair(i, j)\n geom_model.addCollisionPair(col_pair)\n\n\ndef addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel):\n floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0)\n M = pin.SE3.Identity()\n floor_collision_object = pin.GeometryObject(\"floor\", 0, 0, M, floor_collision_shape)\n floor_collision_object.meshColor = np.array([0.5, 0.5, 0.5, 0.5])\n geom_model.addGeometryObject(floor_collision_object)\n\n h = 0.01\n floor_visual_shape = hppfcl.Box(20, 20, h)\n Mvis = pin.SE3.Identity()\n Mvis.translation = np.array([0.0, 0.0, -h / 2])\n floor_visual_object = pin.GeometryObject(\"floor\", 0, 0, Mvis, floor_visual_shape)\n floor_visual_object.meshColor = np.array([0.5, 0.5, 0.5, 0.4])\n visual_model.addGeometryObject(floor_visual_object)\n\n\ndef addMaterialAndCompliance(geom_model, material: str, compliance: float):\n for gobj in geom_model.geometryObjects:\n if material == \"ice\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE\n elif material == \"plastic\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC\n elif material == \"wood\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD\n elif material == \"metal\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL\n elif material == \"concrete\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE\n else:\n raise Exception(\"Material unknown.\")\n\n # Compliance\n gobj.physicsMaterial.compliance = compliance\n\n\ndef printSimulationPerfStats(step_timings: np.ndarray):\n print(\"============================================\")\n print(\"SIMULATION\")\n print(\"Time elapsed: \", np.sum(step_timings))\n print(\n \"Mean timings time step: {mean:.2f} +/- {std:.2f} microseconds\".format(\n mean=np.mean(step_timings) * 1e6, std=np.std(step_timings) * 1e6\n )\n )\n print(\n \"Steps frequency: {freq:.2f} kHz\".format(\n freq=(step_timings.size) / np.sum(step_timings) * 1e-3\n )\n )\n print(\"============================================\")\n\n\ndef runMujocoXML(model_path: str, args: SimulationArgs):\n if not mujoco_imported:\n print(\"Can't run, need to install mujoco\")\n if model_path.endswith(\".xml\"):\n m = mujoco.MjModel.from_xml_path(model_path)\n else:\n m = mlr(model_path)\n m.opt.cone = 1 # Elliptic\n m.opt.solver = 2 # Newton\n m.opt.timestep = args.dt\n m.opt.iterations = args.maxit\n m.opt.tolerance = args.tol\n m.opt.ls_iterations = 50\n m.opt.ls_tolerance = 1e-2\n d = mujoco.MjData(m)\n d.qpos = m.qpos0\n q0 = d.qpos.copy()\n v0 = d.qvel.copy()\n a0 = d.qacc.copy()\n\n print(f\"{q0=}\")\n print(f\"{v0=}\")\n\n step_timings = np.zeros(args.horizon)\n for t in range(args.horizon):\n start_time = time.time()\n mujoco.mj_step(m, d)\n end_time = time.time()\n step_timings[t] = end_time - start_time\n printSimulationPerfStats(step_timings)\n\n d.qpos = q0\n d.qvel = v0\n d.qacc = a0\n show_ui = False\n display_contacts = False\n with mujoco.viewer.launch_passive(\n m, d, show_left_ui=show_ui, show_right_ui=show_ui\n ) as viewer:\n input(\"[Press enter to display trajectory]\")\n while True:\n d.qpos = q0\n d.qvel = v0\n d.qacc = a0\n for t in range(args.horizon):\n step_start = time.time()\n # mj_step can be replaced with code that also evaluates\n # a policy and applies a control signal before stepping the physics.\n mujoco.mj_step(m, d)\n\n # Example modification of a viewer option: toggle contact points every two seconds.\n if display_contacts:\n with viewer.lock():\n viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONSTRAINT] = (\n 1 # int(d.time % 2)\n )\n viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = (\n 1 # int(d.time % 2)\n )\n viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = (\n 1 # int(d.time % 2)\n )\n\n # Pick up changes to the physics state, apply perturbations, update options from GUI.\n viewer.sync()\n\n # Rudimentary time keeping, will drift relative to wall clock.\n time_until_next_step = m.opt.timestep - (time.time() - step_start)\n if args.debug:\n print(d.qpos)\n input(f\"==== TIMESTEP {t} ====\")\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\n\n\ndef createVisualizer(\n model: pin.GeometryModel,\n geom_model: pin.GeometryModel,\n visual_model: pin.GeometryModel,\n):\n viewer = meshcat.Visualizer(zmq_url=\"tcp://127.0.0.1:6000\")\n viewer.delete()\n for obj in visual_model.geometryObjects:\n if obj.name != \"floor\":\n color = np.random.rand(4)\n color[3] = 1.0\n obj.meshColor = color\n vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, visual_model)\n vizer.initViewer(viewer=viewer, open=False, loadModel=True)\n return vizer, viewer\n" }, { "path": "sandbox/simulation_args.py", "content": "import tap\n\n\nclass SimulationArgs(tap.Tap):\n num_repetitions: int = 1\n display: bool = False\n display_com: bool = False\n debug: bool = False\n debug_step: int = 1\n display_collision_model: bool = False\n display_step: bool = False\n display_state: bool = False\n display_contacts: bool = False\n debug_transparency: float = 0.5\n max_fps: int = 120\n Kp: float = 0 # baumgarte proportional term\n Kd: float = 0 # baumgarte derivative term\n compliance: float = 0\n material: str = \"metal\"\n horizon: int = 1000\n dt: float = 1e-3\n tol: float = 1e-6\n tol_rel: float = 1e-6\n mu_prox: float = 1e-4\n maxit: int = 1000\n warm_start: int = 1\n contact_solver: str = \"ADMM\"\n plot_metrics: bool = False\n plot_hist: bool = False\n plot_title: str = \"NO TITLE\"\n seed: int = 1234\n random_initial_velocity: bool = False\n add_damping: bool = False\n damping_factor: float = 0.0\n admm_update_rule: str = \"spectral\"\n mujoco_show_ui: bool = False\n max_patch_size: int = 4\n patch_tolerance: float = 1e-3\n\n def process_args(self):\n if self.debug:\n self.display = True\n self.display_contacts = True\n self.display_state = True\n self.display_com = True\n\n\nclass ControlArgs(SimulationArgs):\n noise_scale: float = 5.0\n nnodes: int = 10 # nnodes = horizon // nsteps\n tau_max: float = 20.0\n wtau: float = 1e-4\n wtarget: float = 5.0\n wvel: float = 1.0\n Nsim: int = 25\n max_fevals: int = 1e4\n use_max_fevals: int = 1\n" }, { "path": "sandbox/simulation_utils.py", "content": "import time\nfrom typing import Dict\nimport numpy as np\nimport tap\nfrom viz_utils import RED, GREEN, BLUE, BLACK, PINK, GREY, BEIGE, PURPLE\nfrom viz_utils import (\n register_line,\n register_arrowed_line,\n register_object,\n transform_object,\n sub_sample,\n)\nimport hppfcl\nimport pinocchio as pin\nimport simple\nimport matplotlib.pyplot as plt\nfrom simulation_args import SimulationArgs\n\n\nclass Policy:\n def __init__(self):\n pass\n\n def act(\n self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt\n ) -> np.ndarray:\n pass\n\n\nclass DefaultPolicy(Policy):\n def __init__(self, model: pin.Model):\n self.actuation = None\n\n def act(\n self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt\n ) -> np.ndarray:\n return np.zeros(simulator.model.nv)\n\n\nclass FreeFloatingRobotDampingPolicy(Policy):\n def __init__(self, model: pin.Model, damping_factor: float):\n self.actuation = np.zeros((model.nv, model.nv - 6))\n self.actuation[6:, :] = np.eye(model.nv - 6)\n self.damping_factor = damping_factor\n\n def act(\n self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt\n ) -> np.ndarray:\n # Note: simulator and model should coincide\n tau_act = -self.damping_factor * v[6:]\n return self.actuation @ tau_act\n\n\nclass RobotArmDampingPolicy(Policy):\n def __init__(self, model: pin.Model, damping_factor: float):\n self.actuation = np.eye(model.nv)\n self.damping_factor = damping_factor\n\n def act(\n self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt\n ) -> np.ndarray:\n # Note: simulator and model should coincide\n tau_act = -self.damping_factor * v\n return self.actuation @ tau_act\n\n\ndef setPhysicsProperties(\n geom_model: pin.GeometryModel, material: str, compliance: float\n):\n for gobj in geom_model.geometryObjects:\n if material == \"ice\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE\n elif material == \"plastic\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC\n elif material == \"wood\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD\n elif material == \"metal\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL\n elif material == \"concrete\":\n gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE\n\n # Compliance\n gobj.physicsMaterial.compliance = compliance\n\n\ndef removeBVHModelsIfAny(geom_model: pin.GeometryModel):\n for gobj in geom_model.geometryObjects:\n gobj: pin.GeometryObject\n bvh_types = [hppfcl.BV_OBBRSS, hppfcl.BV_OBB, hppfcl.BV_AABB]\n ntype = gobj.geometry.getNodeType()\n if ntype in bvh_types:\n gobj.geometry.buildConvexHull(True, \"Qt\")\n gobj.geometry = gobj.geometry.convex\n\n\ndef addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel):\n color = GREY\n color[3] = 0.5\n # Collision object\n # floor_collision_shape = hppfcl.Box(10, 10, 2)\n # M = pin.SE3(np.eye(3), np.zeros(3))\n # M.translation = np.array([0.0, 0.0, -(1.99 / 2.0)])\n floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0)\n # floor_collision_shape = hppfcl.Plane(0, 0, 1, 0)\n # floor_collision_shape.setSweptSphereRadius(0.5)\n M = pin.SE3.Identity()\n floor_collision_object = pin.GeometryObject(\"floor\", 0, 0, M, floor_collision_shape)\n floor_collision_object.meshColor = color\n geom_model.addGeometryObject(floor_collision_object)\n\n # Visual object\n floor_visual_shape = hppfcl.Box(10, 10, 0.01)\n floor_visual_object = pin.GeometryObject(\n \"floor\", 0, 0, pin.SE3.Identity(), floor_visual_shape\n )\n floor_visual_object.meshColor = color\n visual_model.addGeometryObject(floor_visual_object)\n\n\ndef simulateSytem(\n model: pin.Model,\n geom_model: pin.GeometryModel,\n visual_model: pin.GeometryModel,\n q0: np.ndarray,\n v0: np.ndarray,\n policy: Policy,\n args: Dict,\n):\n print(f\"Number of bodies in model = {model.nbodies}\")\n print(\"Num geom obj = \", len(geom_model.geometryObjects))\n if args.debug:\n for i, inertia in enumerate(model.inertias):\n print(\"------------------------\")\n print(f\"Inertia {i} =\\n {inertia}\")\n\n for gobj in geom_model.geometryObjects:\n print(\n f\"name = {gobj.name}, {gobj.geometry.getNodeType()}, parent joint = {gobj.parentJoint}\"\n )\n\n if args.display:\n from pinocchio.visualize import MeshcatVisualizer\n import meshcat\n\n if args.debug:\n if args.display_collision_model:\n rendered_model = geom_model\n else:\n rendered_model = visual_model\n for gobj in rendered_model.geometryObjects:\n color = gobj.meshColor\n if gobj.name == \"floor\":\n color[3] = 0.2\n else:\n color[3] = args.debug_transparency\n gobj.meshColor = color\n\n # visualize the trajectory\n viewer = meshcat.Visualizer(zmq_url=\"tcp://127.0.0.1:6000\")\n viewer.delete()\n viewer[\"/Background\"].set_property(\"top_color\", BEIGE[:3].tolist())\n viewer[\"/Background\"].set_property(\"bottom_color\", BEIGE[:3].tolist())\n viewer[\"/Lights/SpotLight/\"].set_property(\"position\", [-10, -10, -10])\n viewer[\"/Lights/PointLightPositiveX/\"].set_property(\n \"position\", [10, 10, 10]\n )\n vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, visual_model)\n vizer.initViewer(viewer=viewer, open=False, loadModel=True)\n vizer.display(q0)\n if args.display_collision_model:\n vizer.displayCollisions(True)\n vizer.displayVisuals(False)\n else:\n vizer.displayCollisions(False)\n vizer.displayVisuals(True)\n\n data = model.createData()\n geom_data = geom_model.createData()\n\n com = pin.centerOfMass(model, data, q0)\n print(\"Center of mass = \", com)\n if args.display and args.debug and args.display_com:\n sphere_com = hppfcl.Sphere(0.05)\n Mcom = pin.SE3.Identity()\n Mcom.translation = com\n register_object(vizer, sphere_com, \"com\", Mcom, PURPLE)\n\n print(f\"System total mass = {pin.computeTotalMass(model, data)}\")\n print(f\"Armature: {model.armature}\")\n\n for col_req in geom_data.collisionRequests:\n col_req: hppfcl.CollisionRequest\n col_req.security_margin = 0.0\n col_req.break_distance = 0.0\n col_req.gjk_tolerance = 1e-6\n col_req.epa_tolerance = 1e-6\n col_req.gjk_initial_guess = hppfcl.GJKInitialGuess.CachedGuess\n col_req.gjk_variant = hppfcl.GJKVariant.DefaultGJK\n\n for patch_req in geom_data.contactPatchRequests:\n patch_req.setPatchTolerance(args.patch_tolerance)\n\n # Simulation parameters\n if args.contact_solver == \"ADMM\" or args.contact_solver == \"PGS\":\n simulator = simple.Simulator(model, data, geom_model, geom_data)\n # PGS\n simulator.pgs_constraint_solver_settings.absolute_precision = args.tol\n simulator.pgs_constraint_solver_settings.relative_precision = args.tol_rel\n simulator.pgs_constraint_solver_settings.max_iter = args.maxit\n # ADMM\n simulator.admm_constraint_solver_settings.absolute_precision = args.tol\n simulator.admm_constraint_solver_settings.relative_precision = args.tol_rel\n simulator.admm_constraint_solver_settings.max_iter = args.maxit\n simulator.admm_constraint_solver_settings.mu = args.mu_prox\n #\n simulator.warm_start_constraints_forces = args.warm_start\n simulator.measure_timings = True\n # Contact patch settings\n simulator.constraints_problem.setMaxNumberOfContactsPerCollisionPair(\n args.max_patch_size\n )\n # Baumgarte settings\n simulator.constraints_problem.Kp = args.Kp\n simulator.constraints_problem.Kd = args.Kd\n if args.admm_update_rule == \"spectral\":\n simulator.admm_constraint_solver_settings.admm_update_rule = (\n pin.ADMMUpdateRule.SPECTRAL\n )\n elif args.admm_update_rule == \"linear\":\n simulator.admm_constraint_solver_settings.admm_update_rule = (\n pin.ADMMUpdateRule.LINEAR\n )\n else:\n print(f\"ERROR - no match for admm update rule {args.admm_update_rule}\")\n exit(1)\n dt = args.dt\n T = args.horizon\n\n print(\n f\"[Main simulation will be repeated {args.num_repetitions} times to gather timings]\"\n )\n\n if args.display:\n input(\"[Press enter to simulate.]\")\n step_timings = 0\n contact_solver_timings = 0\n for _ in range(args.num_repetitions):\n q = q0.copy()\n v = v0.copy()\n tau = np.zeros(model.nv)\n fext = [pin.Force(np.zeros(6)) for _ in range(model.njoints)]\n simulator.resetBeforeLooping()\n for t in range(T):\n tau = policy.act(simulator, q, v, dt)\n if args.contact_solver == \"ADMM\":\n simulator.step(q, v, tau, fext, dt)\n else:\n simulator.stepPGS(q, v, tau, fext, dt)\n if args.debug and t % args.debug_step == 0:\n if args.display:\n vizer.display(simulator.qnew)\n if args.display_com:\n com = pin.centerOfMass(model, data, simulator.qnew)\n Mcom = pin.SE3.Identity()\n Mcom.translation = com\n transform_object(vizer, sphere_com, \"com\", Mcom)\n\n print(f\"\\n========== TIMESTEP {t} ===========\")\n num_contacts = simulator.constraints_problem.getNumberOfContacts()\n print(f\"===> Num contact points = {num_contacts}\")\n print(\n f\"===> Timings of contact solver = { simulator.getContactSolverCPUTimes().user } us\"\n )\n print(\n f\"===> Timings of step function = { simulator.getStepCPUTimes().user } us\"\n )\n if args.display_state:\n print(f\"===> Joint config [q] =\\n {q}\")\n print(f\"===> Joint vel [v] =\\n {v}\")\n print(f\" ----> v.norm() = {np.linalg.norm(v)}\")\n print(f\"===> Joint torque [tau] =\\n {tau}\")\n print(f\"===> Free joint vel [vfree] =\\n {simulator.vfree}\")\n print(f\"===> Updated joint config [qnew] =\\n {simulator.qnew}\")\n print(f\"===> Updated joint vel [vnew] =\\n {simulator.vnew}\")\n print(f\" ----> vnew.norm() = {np.linalg.norm(simulator.vnew)}\")\n frictional_point_constraints_forces = simulator.constraints_problem.frictional_point_constraints_forces()\n print(\n f\"===> Contact forces [frictional_point_constraints_forces()] =\\n {frictional_point_constraints_forces}\"\n )\n print(\n f\" ----> frictional_point_constraints_forces().norm() = {np.linalg.norm(frictional_point_constraints_forces)}\"\n )\n print(\"===> Total forces (external + contact):\")\n for i in range(model.njoints):\n print(\n f\" ---> {model.names[i]}, ftotal =\\n{simulator.ftotal[i]}\"\n )\n if args.display_contacts:\n vizer.viewer[\"contact_info\"].delete()\n print(\"===> Contact information:\")\n for i in range(num_contacts):\n # Display contact point\n cp = simulator.constraints_problem.contact_points[i]\n sphere = hppfcl.Sphere(0.01)\n cp_name = f\"contact_info/contact_point_{i}\"\n M = pin.SE3.Identity()\n M.translation = cp\n register_object(vizer, sphere, cp_name, M, BLACK)\n\n # Display contact force\n print(\n f\" --> cp {i} = \",\n cp,\n )\n normal = simulator.constraints_problem.contact_normals[i]\n print(\n f\" --> normal {i} = \",\n normal,\n )\n frictional_point_constraints_forces = simulator.constraints_problem.frictional_point_constraints_forces()\n fcontact = frictional_point_constraints_forces[\n 3 * i : 3 * i + 3\n ]\n print(\n f\" --> contact force {i} = {fcontact}\",\n )\n constraint_model: pin.RigidConstraintModel = (\n simulator.constraints_problem.getConstraintModel(i)\n )\n joint1_id = constraint_model.joint1_id\n i1Mc = constraint_model.joint1_placement\n wMc = data.oMi[joint1_id].act(i1Mc)\n spatial_force_loc = pin.Force(fcontact, np.zeros(3))\n spatial_force: pin.Force = wMc.act(spatial_force_loc)\n # To give a nice visual of the contact force, I'll assume the following:\n # 1Kg of force (~10 Newtons) is 0.1 meters -> so we need to divide the contact\n # force by 100.\n visual_factor = 1e-2\n # Note: the - sign is because the normal goes from body 1 to body 2, but we\n # want to view the force exerted by body 2 on body 1.\n # new_cp = cp - normal * fcontact[2] * visual_factor\n new_cp = cp - spatial_force.linear * visual_factor\n force_arrow_name = f\"contact_info/contact_force_{i}\"\n register_arrowed_line(\n vizer, cp, new_cp, force_arrow_name, 0.005, RED\n )\n if args.display_step:\n # Re-print solver info, nice for debugging\n print(f\"===> Num contact points = {num_contacts}\")\n print(\n f\"===> Timings of contact solver = { simulator.getContactSolverCPUTimes().user } us\"\n )\n print(\n f\"===> Timings of step function = { simulator.getStepCPUTimes().user } us\"\n )\n input(f\"[Timestep {t} - press enter to continue.]\")\n\n # Update simulator state\n step_timings += simulator.getStepCPUTimes().user\n contact_solver_timings += simulator.getContactSolverCPUTimes().user\n q = simulator.qnew.copy()\n v = simulator.vnew.copy()\n step_timings *= 1e-6 # convert micro seconds to seconds\n print(\"============================================\")\n print(\"SIMULATION\")\n print(\"Time elapsed: \", step_timings)\n print(\"Mean timings time step: \", step_timings / (T * args.num_repetitions))\n print(\"Steps per second: \", (T * args.num_repetitions) / (step_timings))\n print(\n \"Mean timings contact solver: \",\n contact_solver_timings / (T * args.num_repetitions),\n )\n print(\"============================================\")\n\n if args.display:\n # remove contact info if any\n vizer.viewer[\"contact_info\"].delete()\n\n # recompute and store trajectory\n print(\"[Recomputing trajectory for displaying it...]\")\n q, v = q0.copy(), v0.copy()\n fext = [pin.Force(np.zeros(6)) for _ in range(model.njoints)]\n tau = np.zeros(model.nv)\n qs, vs = [q], [v]\n numit = []\n step_timings = []\n contact_solver_timings = []\n vnorm = []\n contact_forces_norm = []\n num_contacts = []\n mechanical_energy = []\n potential_energy = []\n kinetic_energy = []\n simulator.resetBeforeLooping()\n for t in range(T):\n tau = policy.act(simulator, q, v, dt)\n if args.contact_solver == \"ADMM\":\n simulator.step(q, v, tau, fext, dt)\n else:\n simulator.stepPGS(q, v, tau, fext, dt)\n q = simulator.qnew.copy()\n v = simulator.vnew.copy()\n #\n # Save trajectory for display\n qs.append(q.copy())\n vs.append(v.copy())\n #\n # Save metrics\n vnorm.append(np.linalg.norm(v))\n nc = simulator.constraints_problem.getNumberOfContacts()\n num_contacts.append(nc)\n contact_forces = simulator.constraints_problem.contact_forces()\n contact_forces_norm.append(np.linalg.norm(contact_forces))\n numit.append(simulator.admm_constraint_solver.getIterationCount())\n step_timings.append(simulator.getStepCPUTimes().user)\n contact_solver_timings.append(simulator.getContactSolverCPUTimes().user)\n mechanical_energy.append(pin.computeMechanicalEnergy(model, data, q, v))\n potential_energy.append(pin.computePotentialEnergy(model, data, q))\n kinetic_energy.append(pin.computeKineticEnergy(model, data, q, v))\n\n if args.plot_metrics:\n plt.figure()\n plt.plot(vs, label=[f\"Joint {i}\" for i in range(model.nv)])\n plt.xlabel(\"Timestep\")\n plt.ylabel(\"Joint velocities\")\n plt.legend()\n plt.ion()\n plt.show()\n\n plt.figure()\n plt.plot(mechanical_energy, label=\"Mechanical energy\")\n plt.plot(potential_energy, label=\"Potential energy\")\n plt.plot(kinetic_energy, label=\"Kinetic energy\")\n plt.xlabel(\"Timestep\")\n plt.ylabel(\"Energy\")\n plt.legend()\n plt.ion()\n plt.show()\n\n fontsize = 12\n fig, ax = plt.subplots(nrows=2, ncols=3, figsize=(16, 10))\n if args.plot_hist:\n ax[0, 0].hist(\n numit,\n bins=args.maxit // 2,\n align=\"mid\",\n color=PURPLE,\n density=True,\n histtype=\"bar\",\n cumulative=False,\n linewidth=3,\n rwidth=0.8,\n )\n ax[0, 0].set_xlabel(\"# of iterations\", fontsize=fontsize)\n ax[0, 0].set_ylabel(\"Ratio of problems\", fontsize=fontsize)\n ax[0, 0].set_title(\n f\"{args.contact_solver} solver iterations\",\n fontsize=fontsize,\n )\n else:\n # Number of iterations along trajectory\n ax[0, 0].plot(numit, \"+\", color=PURPLE, linewidth=1)\n ax[0, 0].set_xlabel(\"Timestep\", fontsize=fontsize)\n ax[0, 0].set_ylabel(\"# of iterations\", fontsize=fontsize)\n ax[0, 0].set_title(\n f\"{args.contact_solver} solver iterations along trajectory\",\n fontsize=fontsize,\n )\n\n if args.plot_hist:\n # Distribution of number of contact points\n num_contacts = np.array(num_contacts, dtype=np.int32)\n ax[0, 1].hist(\n num_contacts,\n align=\"mid\",\n color=PURPLE,\n density=True,\n histtype=\"bar\",\n cumulative=False,\n linewidth=3,\n rwidth=0.8,\n )\n ax[0, 1].set_xlabel(\"# contact points\", fontsize=fontsize)\n ax[0, 1].set_ylabel(\"Ratio of problems\", fontsize=fontsize)\n ax[0, 1].set_title(\n \"Distribution # of contact points\",\n fontsize=fontsize,\n )\n else:\n # Number of contacts along trajectory\n ax[0, 1].plot(num_contacts, \"+\", color=PURPLE, linewidth=1)\n ax[0, 1].set_xlabel(\"Timestep\", fontsize=fontsize)\n ax[0, 1].set_ylabel(\"# of contact points\", fontsize=fontsize)\n ax[0, 1].set_title(\n f\"Number of contact points along trajectory\",\n fontsize=fontsize,\n )\n\n # Contact solver timings\n ax[0, 2].plot(contact_solver_timings, \"+\", linewidth=3, color=PURPLE)\n ax[0, 2].set_xlabel(\"Timestep\", fontsize=fontsize)\n ax[0, 2].set_ylabel(\"Contact solver timings\", fontsize=fontsize)\n ax[0, 2].set_title(\n \"Contact solver timings along trajectory\",\n fontsize=fontsize,\n )\n\n # Joint velocity\n ax[1, 0].plot(vnorm, linewidth=3, color=PURPLE)\n ax[1, 0].set_xlabel(\"Timestep\", fontsize=fontsize)\n ax[1, 0].set_ylabel(\"Joint vel norm\", fontsize=fontsize)\n ax[1, 0].set_title(\n \"Joint velocity norm along trajectory\",\n fontsize=fontsize,\n )\n\n # Contact forces\n ax[1, 1].plot(contact_forces_norm, \"+\", linewidth=1, color=PURPLE)\n ax[1, 1].set_xlabel(\"Timestep\", fontsize=fontsize)\n ax[1, 1].set_ylabel(\"Contact forces norm\", fontsize=fontsize)\n ax[1, 1].set_yscale(\"log\")\n ax[1, 1].set_title(\n \"Contact forces norm along trajectory\",\n fontsize=fontsize,\n )\n\n # Step timings\n ax[1, 2].plot(step_timings, \"+\", linewidth=3, color=PURPLE)\n ax[1, 2].set_xlabel(\"Timestep\", fontsize=fontsize)\n ax[1, 2].set_ylabel(\"`Step` timings\", fontsize=fontsize)\n ax[1, 2].set_title(\n \"`Step` timings along trajectory\",\n fontsize=fontsize,\n )\n\n plt.suptitle(\n f\"{args.plot_title}\\ntol = {args.tol}, maxit = {args.maxit}, dt = {args.dt}, horizon = {args.horizon}\"\n )\n plt.ion()\n plt.show()\n\n max_fps = args.max_fps\n fps = min([max_fps, 1.0 / dt])\n dt_vis = 1.0 / float(fps)\n qs = sub_sample(qs, dt * T, fps)\n vizer.display(qs[0])\n input(\"[Press enter to display simulated trajectory]\")\n while True:\n for t in range(len(qs)):\n step_start = time.time()\n vizer.display(qs[t])\n time_until_next_step = dt_vis - (time.time() - step_start)\n if time_until_next_step > 0:\n time.sleep(time_until_next_step)\n" }, { "path": "sandbox/test_memory.py", "content": "import pinocchio as pin\nimport numpy as np\nfrom sim_utils import SimulationArgs, runMujocoXML\nimport mujoco\nimport os\n\nargs = SimulationArgs().parse_args()\nallowed_solvers = [\"ADMM\", \"PGS\"]\nif args.contact_solver not in allowed_solvers:\n print(\n f\"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting\"\n )\n exit(1)\nnp.random.seed(args.seed)\npin.seed(args.seed)\ncurrent_dir = os.path.dirname(os.path.abspath(__file__))\n# model_path = \"./sandbox/robots/humanoid.xml\"\nmodel_path = f\"{current_dir}/robots/go2/mjcf/scene.xml\"\n# runMujocoXML(model_path, args)\n\nn = 1000\nmodels = []\ndatas = []\nfor _ in range(n):\n m = mujoco.MjModel.from_xml_path(model_path)\n m.opt.solver = 0\n d = mujoco.MjData(m)\n # print(d.ncon)\n models.append(m)\n datas.append(d)\n\nfor i in range(n):\n m = models[i]\n d = datas[i]\n mujoco.mj_step(m, d)\n # print(d.ncon)\n\ninput(\"PRESS ENTER TO SWITCH TO PINOCCHIO\")\n\nimport pinocchio as pin\nimport numpy as np\nimport simple\nimport os\n\nfrom simulation_utils import (\n addFloor,\n setPhysicsProperties,\n)\nfrom pin_utils import addSystemCollisionPairs\n\ncurrent_dir = os.path.dirname(os.path.abspath(__file__))\n\n\ndef createSimulator(\n model: pin.Model,\n geom_model: pin.GeometryModel,\n max_num_contacts: int = 4,\n tol: float = 1e-8,\n tol_rel: float = 1e-12,\n mu_prox: float = 1e-4,\n maxit: int = 1000,\n Kp: float = 0.0,\n Kd: float = 0.0,\n):\n data = model.createData()\n geom_data = geom_model.createData()\n simulator = simple.Simulator(model, data, geom_model, geom_data)\n simulator.admm_constraint_solver_settings.absolute_precision = tol\n simulator.admm_constraint_solver_settings.relative_precision = tol_rel\n simulator.admm_constraint_solver_settings.max_iter = maxit\n simulator.admm_constraint_solver_settings.mu = mu_prox\n simulator.constraints_problem.setMaxNumberOfContactsPerCollisionPair(\n max_num_contacts\n )\n simulator.constraints_problem.Kp = Kp\n simulator.constraints_problem.Kd = Kd\n return simulator\n\n\nmodel = pin.buildModelFromMJCF(f\"{current_dir}/robots/go2/mjcf/go2.xml\")\ngeom_model = pin.buildGeomFromMJCF(\n model, f\"{current_dir}/robots/go2/mjcf/go2.xml\", pin.COLLISION\n)\nprint(\"Done loading pinocchio model\")\n\nmaterial = \"concrete\"\ncompliance = 0.0\n\nvisual_model = geom_model.copy()\naddFloor(geom_model, visual_model)\nsetPhysicsProperties(geom_model, material, compliance)\n\nq = pin.neutral(model)\nq[2] = 0.5\nv = np.zeros(model.nv)\naddSystemCollisionPairs(model, geom_model, q)\n\nprint(\"Creating simple simulators...\")\nsimulators = []\nfor _ in range(n):\n sim = createSimulator(model, geom_model)\n simulators.append(sim)\ninput(\"EOF\")\n" }, { "path": "sandbox/viz_utils.py", "content": "import hppfcl\nimport numpy as np\nimport meshcat\nimport meshcat.geometry as mg\nimport pinocchio as pin\nfrom pinocchio.visualize import MeshcatVisualizer\nimport warnings\nfrom typing import Any, Dict, Union, List\n\nMsgType = Dict[str, Union[str, bytes, bool, float, \"MsgType\"]]\n\nRED = np.array([232, 114, 84, 255]) / 255\nGREEN = np.array([84, 232, 121, 255]) / 255\nBLUE = np.array([96, 86, 232, 255]) / 255\nBLACK = np.array([58, 60, 69, 255]) / 255\nPINK = np.array([239, 47, 201, 255]) / 255\nGREY = np.array([192, 201, 229, 255]) / 255\nBEIGE = np.array([252, 247, 234, 255]) / 255\nPURPLE = np.array([161, 34, 183, 255]) / 255\n\n\ndef sub_sample(xs, duration, fps):\n nb_frames = len(xs)\n nb_subframes = int(duration * fps)\n if nb_frames < nb_subframes:\n return xs\n else:\n step = nb_frames // nb_subframes\n xs_sub = [xs[i] for i in range(0, nb_frames, step)]\n return xs_sub\n\n\ndef rgbToHex(color):\n if len(color) == 4:\n c = color[:3]\n opacity = color[3]\n else:\n c = color\n opacity = 1.0\n hex_color = \"0x%02x%02x%02x\" % (int(c[0] * 255), int(c[1] * 255), int(c[2] * 255))\n return hex_color, opacity\n\n\ndef register_object(\n viz: MeshcatVisualizer,\n shape: hppfcl.ShapeBase,\n shape_name: str,\n M: pin.SE3,\n shape_color=np.ones(4),\n) -> int:\n meshcat_shape = load_primitive(shape)\n if isinstance(shape, (hppfcl.Plane, hppfcl.Halfspace)):\n T = M.copy()\n T.translation += M.rotation @ (shape.d * shape.n)\n T = T.homogeneous\n else:\n T = M.homogeneous\n\n # Update viewer configuration.\n viz.viewer[shape_name].set_object(meshcat_shape, meshcat_material(*shape_color))\n viz.viewer[shape_name].set_transform(T)\n\n\ndef register_line(\n viz: MeshcatVisualizer,\n pt1: np.ndarray,\n pt2: np.ndarray,\n line_name: str,\n linewidth: float = 0.01,\n color: np.ndarray = BLACK,\n) -> int:\n height = np.linalg.norm(pt2 - pt1)\n if height > 1e-6:\n axis_ref = np.array([0.0, 0.0, 1.0])\n axis = (pt2 - pt1) / height # - np.array([0., 0., 1.])\n num = np.outer(axis_ref + axis, axis_ref + axis)\n den = np.dot(axis_ref + axis, axis_ref + axis)\n if den > 1e-6:\n R = 2 * num / den - np.eye(3)\n else:\n R = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])\n t = pt1\n translation = pin.SE3(np.eye(3), np.array([0.0, 0.0, height / 2]))\n M = pin.SE3(R, t)\n Mtot = M * translation\n cylinder = hppfcl.Cylinder(linewidth, height)\n meshcat_shape = load_primitive(cylinder)\n viz.viewer[line_name].set_object(meshcat_shape, meshcat_material(*color))\n viz.viewer[line_name].set_transform(Mtot.homogeneous)\n\n\ndef register_arrowed_line(\n viz: MeshcatVisualizer,\n pt1: np.ndarray,\n pt2: np.ndarray,\n line_name: str,\n linewidth: float = 0.01,\n color: np.ndarray = BLACK,\n) -> int:\n height = np.linalg.norm(pt2 - pt1)\n if height > 1e-6:\n register_line(viz, pt1, pt2, line_name, linewidth, color)\n arrow: hppfcl.Convex = create_arrow_head(4 * linewidth)\n M = pin.SE3(np.eye(3), np.array([0.0, 0.0, height / 2]))\n arrow_shape = load_primitive(arrow)\n arrow_name = line_name + \"/arrow\"\n viz.viewer[arrow_name].set_object(arrow_shape, meshcat_material(*color))\n viz.viewer[arrow_name].set_transform(M.homogeneous)\n\n\ndef transform_object(\n viz: MeshcatVisualizer, shape: hppfcl.ShapeBase, shape_name: str, M: pin.SE3\n) -> None:\n if isinstance(shape, (hppfcl.Plane, hppfcl.Halfspace)):\n T = M.copy()\n T.translation += M.rotation @ (shape.d * shape.n)\n T = T.homogeneous\n else:\n T = M.homogeneous\n\n # Update viewer configuration.\n viz.viewer[shape_name].set_transform(T)\n return\n\n\ndef delete_object(viz: MeshcatVisualizer, name: str):\n viz.viewer[name].delete()\n\n\nTWOPI = 2 * np.pi\n\n\ndef create_arrow_head(scale_: float = 1.0, n: int = 10) -> hppfcl.Convex:\n scale = scale_ / 2\n pts = hppfcl.StdVec_Vec3f()\n assert n > 3\n center = np.zeros(3)\n for i in range(n):\n pt = scale * np.array([np.cos(TWOPI * i / n), np.sin(TWOPI * i / n), 0.0])\n center += pt\n pts.append(pt)\n pts.append(center)\n pts.append(scale * np.array([0.0, 0.0, 2.0]))\n\n tris = hppfcl.StdVec_Triangle()\n for i in range(n):\n # Base triangle\n tris.append(hppfcl.Triangle(i, n, (i + 1) % n))\n # Side triangle\n tris.append(hppfcl.Triangle(i, (i + 1) % n, n + 1))\n\n cvx = hppfcl.Convex(pts, tris)\n return cvx\n\n\ndef npToTTuple(M):\n L = M.tolist()\n for i in range(len(L)):\n L[i] = tuple(L[i])\n return tuple(L)\n\n\ndef npToTuple(M):\n if len(M.shape) == 1:\n return tuple(M.tolist())\n if M.shape[0] == 1:\n return tuple(M.tolist()[0])\n if M.shape[1] == 1:\n return tuple(M.T.tolist()[0])\n return npToTTuple(M)\n\n\ndef load_primitive(geom: hppfcl.ShapeBase):\n import meshcat.geometry as mg\n\n # Cylinders need to be rotated\n basic_three_js_transform = np.array(\n [\n [1.0, 0.0, 0.0, 0.0],\n [0.0, 0.0, -1.0, 0.0],\n [0.0, 1.0, 0.0, 0.0],\n [0.0, 0.0, 0.0, 1.0],\n ]\n )\n RotatedCylinder = type(\n \"RotatedCylinder\",\n (mg.Cylinder,),\n {\"intrinsic_transform\": lambda self: basic_three_js_transform},\n )\n\n # Cones need to be rotated\n\n if isinstance(geom, hppfcl.Capsule):\n if hasattr(mg, \"TriangularMeshGeometry\"):\n obj = createCapsule(2.0 * geom.halfLength, geom.radius)\n else:\n obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius)\n elif isinstance(geom, hppfcl.Cylinder):\n obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius)\n elif isinstance(geom, hppfcl.Cone):\n obj = RotatedCylinder(2.0 * geom.halfLength, 0, geom.radius, 0)\n elif isinstance(geom, hppfcl.Box):\n obj = mg.Box(npToTuple(2.0 * geom.halfSide))\n elif isinstance(geom, hppfcl.Sphere):\n obj = mg.Sphere(geom.radius)\n elif isinstance(geom, hppfcl.TriangleP):\n faces = np.empty((1, 3), dtype=int)\n vertices = np.empty((3, 3))\n vertices[0] = geom.a\n vertices[1] = geom.b\n vertices[2] = geom.c\n faces[0] = [0, 1, 2]\n obj = mg.TriangularMeshGeometry(vertices, faces)\n elif isinstance(geom, hppfcl.Ellipsoid):\n obj = mg.Ellipsoid(geom.radii)\n elif isinstance(geom, (hppfcl.Plane, hppfcl.Halfspace)):\n plane_transform: pin.SE3 = pin.SE3.Identity()\n # plane_transform.translation[:] = geom.d # Does not work\n plane_transform.rotation = pin.Quaternion.FromTwoVectors(\n geom.n, pin.ZAxis\n ).toRotationMatrix()\n TransformedPlane = type(\n \"TransformedPlane\",\n (Plane,),\n {\"intrinsic_transform\": lambda self: plane_transform.homogeneous},\n )\n obj = TransformedPlane(10, 10)\n elif isinstance(geom, hppfcl.ConvexBase):\n obj = loadMesh(geom)\n else:\n msg = \"Unsupported geometry type for (%s)\" % (type(geom))\n warnings.warn(msg, category=UserWarning, stacklevel=2)\n obj = None\n\n return obj\n\n\ndef loadMesh(mesh):\n if isinstance(mesh, (hppfcl.Convex, hppfcl.BVHModelBase)):\n if isinstance(mesh, hppfcl.BVHModelBase):\n num_vertices = mesh.num_vertices\n num_tris = mesh.num_tris\n\n call_triangles = mesh.tri_indices\n call_vertices = mesh.vertices\n\n elif isinstance(mesh, hppfcl.Convex):\n num_vertices = mesh.num_points\n num_tris = mesh.num_polygons\n\n call_triangles = mesh.polygons\n call_vertices = mesh.points\n\n faces = np.empty((num_tris, 3), dtype=int)\n for k in range(num_tris):\n tri = call_triangles(k)\n faces[k] = [tri[i] for i in range(3)]\n\n vertices = call_vertices()\n\n vertices = vertices.astype(np.float32)\n\n if num_tris > 0:\n mesh = mg.TriangularMeshGeometry(vertices, faces)\n else:\n mesh = mg.Points(\n mg.PointsGeometry(\n vertices.T, color=np.repeat(np.ones((3, 1)), num_vertices, axis=1)\n ),\n mg.PointsMaterial(size=0.002),\n )\n\n return mesh\n\n\ndef createCapsule(length, radius, radial_resolution=30, cap_resolution=10):\n nbv = np.array([max(radial_resolution, 4), max(cap_resolution, 4)])\n h = length\n r = radius\n position = 0\n vertices = np.zeros((nbv[0] * (2 * nbv[1]) + 2, 3))\n for j in range(nbv[0]):\n phi = (2 * np.pi * j) / nbv[0]\n for i in range(nbv[1]):\n theta = (np.pi / 2 * i) / nbv[1]\n vertices[position + i, :] = np.array(\n [\n np.cos(theta) * np.cos(phi) * r,\n np.cos(theta) * np.sin(phi) * r,\n -h / 2 - np.sin(theta) * r,\n ]\n )\n vertices[position + i + nbv[1], :] = np.array(\n [\n np.cos(theta) * np.cos(phi) * r,\n np.cos(theta) * np.sin(phi) * r,\n h / 2 + np.sin(theta) * r,\n ]\n )\n position += nbv[1] * 2\n vertices[-2, :] = np.array([0, 0, -h / 2 - r])\n vertices[-1, :] = np.array([0, 0, h / 2 + r])\n indexes = np.zeros((nbv[0] * (4 * (nbv[1] - 1) + 4), 3))\n index = 0\n stride = nbv[1] * 2\n last = nbv[0] * (2 * nbv[1]) + 1\n for j in range(nbv[0]):\n j_next = (j + 1) % nbv[0]\n indexes[index + 0] = np.array(\n [j_next * stride + nbv[1], j_next * stride, j * stride]\n )\n indexes[index + 1] = np.array(\n [j * stride + nbv[1], j_next * stride + nbv[1], j * stride]\n )\n indexes[index + 2] = np.array(\n [j * stride + nbv[1] - 1, j_next * stride + nbv[1] - 1, last - 1]\n )\n indexes[index + 3] = np.array(\n [j_next * stride + 2 * nbv[1] - 1, j * stride + 2 * nbv[1] - 1, last]\n )\n for i in range(nbv[1] - 1):\n indexes[index + 4 + i * 4 + 0] = np.array(\n [j_next * stride + i, j_next * stride + i + 1, j * stride + i]\n )\n indexes[index + 4 + i * 4 + 1] = np.array(\n [j_next * stride + i + 1, j * stride + i + 1, j * stride + i]\n )\n indexes[index + 4 + i * 4 + 2] = np.array(\n [\n j_next * stride + nbv[1] + i + 1,\n j_next * stride + nbv[1] + i,\n j * stride + nbv[1] + i,\n ]\n )\n indexes[index + 4 + i * 4 + 3] = np.array(\n [\n j_next * stride + nbv[1] + i + 1,\n j * stride + nbv[1] + i,\n j * stride + nbv[1] + i + 1,\n ]\n )\n index += 4 * (nbv[1] - 1) + 4\n return mg.TriangularMeshGeometry(vertices, indexes)\n\n\nclass Plane(mg.Geometry):\n \"\"\"A plane of the given width and height.\"\"\"\n\n def __init__(\n self,\n width: float,\n height: float,\n widthSegments: float = 1,\n heightSegments: float = 1,\n ):\n super().__init__()\n self.width = width\n self.height = height\n self.widthSegments = widthSegments\n self.heightSegments = heightSegments\n\n def lower(self, object_data: Any) -> MsgType:\n return {\n \"uuid\": self.uuid,\n \"type\": \"PlaneGeometry\",\n \"width\": self.width,\n \"height\": self.height,\n \"widthSegments\": self.widthSegments,\n \"heightSegments\": self.heightSegments,\n }\n\n\ndef meshcat_material(r, g, b, a):\n material = mg.MeshPhongMaterial()\n material.color = int(r * 255) * 256**2 + int(g * 255) * 256 + int(b * 255)\n material.opacity = a\n return material\n\n\ndef create_visualizer(\n grid: bool = False, axes: bool = False, zmq_url=\"tcp://127.0.0.1:6000\"\n) -> meshcat.Visualizer:\n vis = meshcat.Visualizer(zmq_url=zmq_url)\n # vis = meshcat.Visualizer()\n vis.delete()\n if not grid:\n vis[\"/Grid\"].set_property(\"visible\", False)\n if not axes:\n vis[\"/Axes\"].set_property(\"visible\", False)\n return vis\n" }, { "path": "sources.cmake", "content": "# Define Simple sources and headers\n\n# Core library\nset(${PROJECT_NAME}_CORE_SOURCES empty.cpp)\n\nset(${PROJECT_NAME}_CORE_PUBLIC_HEADERS\n ${PROJECT_SOURCE_DIR}/include/simple/utils/visitors.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/core/fwd.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/core/contact-frame.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/core/contact-frame.hxx\n ${PROJECT_SOURCE_DIR}/include/simple/core/constraints-problem.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/core/constraints-problem.hxx\n ${PROJECT_SOURCE_DIR}/include/simple/core/simulator.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/core/simulator.hxx\n ${PROJECT_SOURCE_DIR}/include/simple/math/fwd.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/math/qr.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/fwd.hpp)\n\nset(_binary_headers_root ${${PROJECT_NAME}_BINARY_DIR}/include/simple)\nset(${PROJECT_NAME}_CORE_GENERATED_PUBLIC_HEADERS\n ${_binary_headers_root}/config.hpp ${_binary_headers_root}/deprecated.hpp\n ${_binary_headers_root}/warning.hpp)\n\n# Template instantiation\nset(${PROJECT_NAME}_TEMPLATE_INSTANTIATION_PUBLIC_HEADERS\n ${PROJECT_SOURCE_DIR}/include/simple/core/constraints-problem.txx\n ${PROJECT_SOURCE_DIR}/include/simple/core/simulator.txx)\n\nset(${PROJECT_NAME}_TEMPLATE_INSTANTIATION_SOURCES core/constraints-problem.cpp core/simulator.cpp)\n\nset(${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_SOURCES\n pinocchio_template_instantiation/aba-derivatives.cpp pinocchio_template_instantiation/aba.cpp\n pinocchio_template_instantiation/crba.cpp pinocchio_template_instantiation/joint-model.cpp)\n\nset(${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_HEADERS\n ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/aba-derivatives.txx\n ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/aba.txx\n ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/crba.txx\n ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/joint-model.txx)\n\n# Python bindings\nset(${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS\n ${PROJECT_SOURCE_DIR}/include/simple/bindings/python/fwd.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/bindings/python/core/constraints-problem.hpp\n ${PROJECT_SOURCE_DIR}/include/simple/bindings/python/core/simulator.hpp)\n\nset(${PROJECT_NAME}_BINDINGS_PYTHON_SOURCES\n ${PROJECT_SOURCE_DIR}/bindings/python/core/expose-contact-frame.cpp\n ${PROJECT_SOURCE_DIR}/bindings/python/core/expose-constraints-problem.cpp\n ${PROJECT_SOURCE_DIR}/bindings/python/core/expose-simulator.cpp\n ${PROJECT_SOURCE_DIR}/bindings/python/module.cpp)\n" }, { "path": "src/CMakeLists.txt", "content": "#\n# Copyright (c) 2024 INRIA\n#\n\n# Create header-only target All other target will depend on it.\nadd_library(${PROJECT_NAME}_header_only INTERFACE ${${PROJECT_NAME}_HEADERS})\n# On CMake 3.16, we can't target_sources(${PROJECT_NAME}_header_only INTERFACE\n# ${${PROJECT_NAME}_CORE_PUBLIC_HEADERS})\n\nif(INITIALIZE_WITH_NAN)\n target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE EIGEN_INITIALIZE_MATRICES_BY_NAN)\nendif()\n\nif(CHECK_RUNTIME_MALLOC)\n target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE SIMPLE_EIGEN_CHECK_MALLOC\n EIGEN_RUNTIME_NO_MALLOC)\nendif(CHECK_RUNTIME_MALLOC)\nif(ENABLE_TEMPLATE_INSTANTIATION)\n target_compile_definitions(${PROJECT_NAME}_header_only\n INTERFACE SIMPLE_ENABLE_TEMPLATE_INSTANTIATION)\nendif()\n\ntarget_link_libraries(${PROJECT_NAME}_header_only INTERFACE Eigen3::Eigen pinocchio::pinocchio\n Boost::boost Boost::serialization)\n# add precompiled header for pinocchio and eigen headers\nif(USE_PRECOMPILED_HEADERS)\n target_precompile_headers(${PROJECT_NAME}_header_only INTERFACE\n ${PROJECT_SOURCE_DIR}/include/simple/pch.hpp)\nendif()\n\n# Enable diffcoal for collision detection derivatives\nif(SIMPLE_USE_DIFFCOAL)\n target_link_libraries(${PROJECT_NAME}_header_only INTERFACE diffcoal::diffcoal)\nelse(SIMPLE_USE_DIFFCOAL)\n target_compile_definitions(${PROJECT_NAME}_header_only\n INTERFACE SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS)\nendif(SIMPLE_USE_DIFFCOAL)\n\n# Enable tracy profiling\nif(SIMPLE_TRACY_ENABLE)\n modernize_target_link_libraries(\n ${PROJECT_NAME}_header_only\n SCOPE INTERFACE\n TARGETS Tracy::TracyClient)\nendif()\n\ntarget_include_directories(${PROJECT_NAME}_header_only INTERFACE $)\n\ncxx_flags_by_compiler_frontend(\n MSVC \"/bigobj\"\n OUTPUT PUBLIC_OPTIONS\n FILTER)\ntarget_compile_options(${PROJECT_NAME}_header_only INTERFACE ${PUBLIC_OPTIONS})\n\ncxx_flags_by_compiler_frontend(MSVC \"NOMINMAX\" OUTPUT PUBLIC_DEFINITIONS)\ntarget_compile_definitions(${PROJECT_NAME}_header_only INTERFACE ${PUBLIC_DEFINITIONS})\n\nadd_header_group(${PROJECT_NAME}_CORE_PUBLIC_HEADERS)\nadd_header_group(${PROJECT_NAME}_CORE_GENERATED_PUBLIC_HEADERS)\n\ninstall(\n TARGETS ${PROJECT_NAME}_header_only\n EXPORT ${TARGETS_EXPORT_NAME}\n PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}\n INCLUDES\n DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}\n LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}\n ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}\n RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})\n\nif(ENABLE_TEMPLATE_INSTANTIATION)\n add_library(\n ${PROJECT_NAME}_pinocchio_ti SHARED ${${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_SOURCES}\n ${${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_HEADERS})\n target_link_libraries(${PROJECT_NAME}_pinocchio_ti PUBLIC ${PROJECT_NAME}_header_only)\n\n install(\n TARGETS ${PROJECT_NAME}_pinocchio_ti\n EXPORT ${TARGETS_EXPORT_NAME}\n PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}\n INCLUDES\n DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}\n LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}\n ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}\n RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})\nendif()\n\nadd_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_CORE_SOURCES})\ntarget_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_header_only)\nif(ENABLE_TEMPLATE_INSTANTIATION)\n target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_pinocchio_ti)\nendif()\n\ninstall(\n TARGETS ${PROJECT_NAME}\n EXPORT ${TARGETS_EXPORT_NAME}\n PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}\n INCLUDES\n DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}\n LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}\n ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}\n RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})\n" }, { "path": "src/core/constraints-problem.cpp", "content": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#include \"simple/core/constraints-problem.hpp\"\n\nnamespace simple\n{\n\n template struct ConstraintsProblemTpl;\n\n} // namespace simple\n" }, { "path": "src/core/simulator.cpp", "content": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n\nnamespace simple\n{\n template struct SimulatorTpl;\n\n template void\n SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n context::Scalar);\n\n template void\n SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &,\n context::Scalar);\n\n template void\n SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n context::Scalar);\n\n template void\n SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>(\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &,\n context::Scalar);\n\n template void SimulatorTpl::resolveConstraints<\n ::pinocchio::ADMMContactSolverTpl>(const Eigen::MatrixBase &, const Scalar);\n\n template void SimulatorTpl::resolveConstraints<\n ::pinocchio::PGSContactSolverTpl>(const Eigen::MatrixBase &, const Scalar);\n\n} // namespace simple\n" }, { "path": "src/empty.cpp", "content": "" }, { "path": "src/pinocchio_template_instantiation/aba-derivatives.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/fwd.hpp\"\n#include \n\ntemplate PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void ::pinocchio::computeABADerivatives<\n ::simple::context::Scalar,\n ::simple::context::Options,\n ::pinocchio::JointCollectionDefaultTpl,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::Force,\n Eigen::aligned_allocator<::simple::context::Force>,\n Eigen::Ref<::simple::context::RowMatrixXs>,\n Eigen::Ref<::simple::context::RowMatrixXs>,\n Eigen::Ref<::simple::context::RowMatrixXs>>(\n const ::simple::context::Model &,\n ::simple::context::Data &,\n const Eigen::MatrixBase<::simple::context::VectorXs> &,\n const Eigen::MatrixBase<::simple::context::VectorXs> &,\n const Eigen::MatrixBase<::simple::context::VectorXs> &,\n const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &,\n const Eigen::MatrixBase> &,\n const Eigen::MatrixBase> &,\n const Eigen::MatrixBase> &);\n" }, { "path": "src/pinocchio_template_instantiation/aba.cpp", "content": "#include \"simple/fwd.hpp\"\n#include \n\ntemplate PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba<\n ::simple::context::Scalar,\n ::simple::context::Options,\n ::pinocchio::JointCollectionDefaultTpl,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs>(\n const ::simple::context::Model &,\n ::simple::context::Data &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Convention);\n\ntemplate PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba<\n ::simple::context::Scalar,\n ::simple::context::Options,\n ::pinocchio::JointCollectionDefaultTpl,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::VectorXs,\n ::simple::context::Force,\n Eigen::aligned_allocator<::simple::context::Force>>(\n const ::simple::context::Model &,\n ::simple::context::Data &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const Eigen::MatrixBase &,\n const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &,\n const Convention);\n" }, { "path": "src/pinocchio_template_instantiation/crba.cpp", "content": "#include \"simple/fwd.hpp\"\n#include \n\ntemplate PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::MatrixXs & ::pinocchio::\n crba<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs>(\n const context::Model &, context::Data &, const Eigen::MatrixBase &, const Convention convention);\n" }, { "path": "src/pinocchio_template_instantiation/joint-model.cpp", "content": "#include \"simple/fwd.hpp\"\n#include \"pinocchio/multibody/joint/joint-generic.hpp\"\n\ntemplate struct ::pinocchio::JointModelTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>;\n\ntemplate struct ::pinocchio::JointDataTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>;\n" }, { "path": "tests/CMakeLists.txt", "content": "#\n# Copyright (c) 2024 INRIA\n#\n\n# Find Boost.UnitTestFramework\nfind_package(Boost COMPONENTS unit_test_framework)\n\n# Compute flags outside the macro to avoid recomputing it for each tests\ncxx_flags_by_compiler_frontend(MSVC _USE_MATH_DEFINES OUTPUT TEST_PRIVATE_DEFINITIONS)\n\nset(SIMPLE_UNIT_TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR})\n\nfunction(GET_CPP_TEST_NAME name src_dir full_test_name)\n string(REPLACE \"${SIMPLE_UNIT_TEST_DIR}\" \"\" prefix_name \"${src_dir}\")\n string(REGEX REPLACE \"[/]\" \"-\" prefix_name \"${prefix_name}-\")\n set(${full_test_name}\n \"${PROJECT_NAME}-test-cpp${prefix_name}${name}\"\n PARENT_SCOPE)\nendfunction()\n\nfunction(ADD_SIMPLE_UNIT_TEST name)\n set(oneValueArgs)\n set(multiValueArgs PACKAGES)\n cmake_parse_arguments(unit_test \"${options}\" \"${oneValueArgs}\" \"${multiValueArgs}\" ${ARGN})\n\n set(PKGS ${unit_test_PACKAGES})\n\n get_cpp_test_name(${name} ${CMAKE_CURRENT_SOURCE_DIR} TEST_NAME)\n add_unit_test(${TEST_NAME} ${name}.cpp)\n\n set(MODULE_NAME \"Test\")\n string(REPLACE \"-\" \"_\" MODULE_NAME ${MODULE_NAME})\n target_compile_definitions(\n ${TEST_NAME} PRIVATE ${TEST_PRIVATE_DEFINITIONS} BOOST_TEST_DYN_LINK\n BOOST_TEST_MODULE=${MODULE_NAME} SIMPLE_MODEL_DIR=\\\"${SIMPLE_MODEL_DIR}\\\")\n\n # There is no RPATH in Windows, then we must use the PATH to find the DLL\n if(WIN32)\n string(REPLACE \";\" \"\\\\\\;\" _PATH \"$ENV{PATH}\")\n set(ENV_VARIABLES\n \"PATH=${_PATH}\\\\\\;${CMAKE_BINARY_DIR}/src\\\\\\;${CMAKE_BINARY_DIR}/bindings/python/simple\")\n set_tests_properties(${TEST_NAME} PROPERTIES ENVIRONMENT \"${ENV_VARIABLES}\")\n endif()\n\n set_target_properties(${TEST_NAME} PROPERTIES LINKER_LANGUAGE CXX)\n target_include_directories(${TEST_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})\n target_link_libraries(${TEST_NAME} PUBLIC ${PROJECT_NAME})\n # target_link_libraries(${TEST_NAME} PUBLIC Eigen3::Eigen)\n target_link_libraries(${TEST_NAME} PRIVATE Boost::unit_test_framework)\n\n if(PKGS)\n target_link_libraries(${TEST_NAME} PRIVATE ${PKGS})\n endif()\nendfunction()\n\n# CPP test\nadd_subdirectory(forward)\n\n# Python tests\nif(BUILD_PYTHON_INTERFACE)\n file(GLOB_RECURSE ${PROJECT_NAME}_PYTHON_UNITTEST *.py)\n\n foreach(TEST_FILE ${${PROJECT_NAME}_PYTHON_UNITTEST})\n get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE)\n string(REGEX REPLACE \"${PROJECT_SOURCE_DIR}/\" \"\" TEST_FILE ${TEST_FILE})\n add_python_unit_test(\"${PROJECT_NAME}-test-py-${TEST_NAME}\" \"${TEST_FILE}\" \"bindings\")\n endforeach(TEST_FILE ${${PROJECT_NAME}_PYTHON_UNITTEST})\nendif(BUILD_PYTHON_INTERFACE)\n" }, { "path": "tests/forward/CMakeLists.txt", "content": "add_simple_unit_test(mujoco-humanoid)\nadd_simple_unit_test(simulation-combine-constraints)\nadd_simple_unit_test(simulation-robots)\nadd_simple_unit_test(simulator-minimal)\nadd_simple_unit_test(simulator)\n# add_simple_unit_test(urdf-romeo)\n\n# TODO: TEST - Romeo not found\n" }, { "path": "tests/forward/mujoco-humanoid.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \n\n#include \n\n#include \n#include \n#include \n\n#include \"tests/test_data/config.h\"\n#include \"../test-utils.hpp\"\n\nusing namespace simple;\nusing namespace pinocchio;\nusing ModelHandle = Simulator::ModelHandle;\nusing DataHandle = Simulator::DataHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\nusing GeometryDataHandle = Simulator::GeometryDataHandle;\n#define ADMM ::pinocchio::ADMMContactSolverTpl\n#define PGS ::pinocchio::PGSContactSolverTpl\n\nBOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)\n\nvoid addFloorToGeomModel(GeometryModel & geom_model)\n{\n using CollisionGeometryPtr = GeometryObject::CollisionGeometryPtr;\n\n CollisionGeometryPtr floor_collision_shape = CollisionGeometryPtr(new hpp::fcl::Halfspace(0.0, 0.0, 1.0, 0.0));\n\n const SE3 M = SE3::Identity();\n GeometryObject floor(\"floor\", 0, 0, M, floor_collision_shape);\n geom_model.addGeometryObject(floor);\n}\n\nvoid addSystemCollisionPairs(const Model & model, GeometryModel & geom_model, const Eigen::VectorXd & qref)\n{\n Data data(model);\n GeometryData geom_data(geom_model);\n // TI this function to gain compilation speed on this test\n ::pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, qref);\n geom_model.removeAllCollisionPairs();\n for (std::size_t i = 0; i < geom_model.geometryObjects.size(); ++i)\n {\n for (std::size_t j = i; j < geom_model.geometryObjects.size(); ++j)\n {\n if (i == j)\n {\n continue; // don't add collision pair if same object\n }\n const GeometryObject & gobj_i = geom_model.geometryObjects[i];\n const GeometryObject & gobj_j = geom_model.geometryObjects[j];\n if (gobj_i.name == \"floor\" || gobj_j.name == \"floor\")\n { // if floor, always add a collision pair\n geom_model.addCollisionPair(CollisionPair(i, j));\n }\n else\n {\n if (gobj_i.parentJoint == gobj_j.parentJoint)\n { // don't add collision pair if same parent\n continue;\n }\n\n // run collision detection -- add collision pair if shapes are not colliding\n const SE3 M1 = geom_data.oMg[i];\n const SE3 M2 = geom_data.oMg[j];\n\n hpp::fcl::CollisionRequest colreq;\n colreq.security_margin = 1e-2; // 1cm of clearance\n hpp::fcl::CollisionResult colres;\n hpp::fcl::collide(\n gobj_i.geometry.get(), ::pinocchio::toFclTransform3f(M1), //\n gobj_j.geometry.get(), ::pinocchio::toFclTransform3f(M2), //\n colreq, colres);\n if (!colres.isCollision())\n {\n geom_model.addCollisionPair(CollisionPair(i, j));\n }\n }\n }\n }\n}\n\nBOOST_AUTO_TEST_CASE(mujoco_humanoid)\n{\n ModelHandle model_handle(new Model());\n Model & model = ::pinocchio::helper::get_ref(model_handle);\n GeometryModelHandle geom_model_handle(new GeometryModel());\n GeometryModel & geom_model = ::pinocchio::helper::get_ref(geom_model_handle);\n\n const bool verbose = false;\n ::pinocchio::mjcf::buildModel(SIMPLE_TEST_DATA_DIR \"/mujoco_humanoid.xml\", model, verbose);\n ::pinocchio::mjcf::buildGeom(model, SIMPLE_TEST_DATA_DIR \"/mujoco_humanoid.xml\", pinocchio::COLLISION, geom_model);\n addFloorToGeomModel(geom_model);\n\n // sanity checks\n assert(model.nv == 27);\n assert(geom_model.geometryObjects.size() == 20);\n\n // initial state\n const Eigen::VectorXd q0 = model.referenceConfigurations[\"qpos0\"];\n const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv);\n const Eigen::VectorXd tau0 = Eigen::VectorXd::Zero(model.nv);\n\n // add collision pairs\n addSystemCollisionPairs(model, geom_model, q0);\n assert(geom_model.collisionPairs.size() == 175);\n\n // run simulation\n model.lowerPositionLimit.setConstant(-std::numeric_limits::infinity());\n model.upperPositionLimit.setConstant(std::numeric_limits::infinity());\n model.lowerDryFrictionLimit.setZero();\n model.upperDryFrictionLimit.setZero();\n const double dt = 1e-3;\n const Eigen::VectorXd zero_torque = Eigen::VectorXd::Zero(model.nv);\n\n {\n Simulator sim(model_handle, geom_model_handle);\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = v0;\n for (size_t i = 0; i < 100; ++i)\n {\n BOOST_CHECK_NO_THROW(sim.step(q, v, zero_torque, dt));\n BOOST_CHECK(sim.admm_constraint_solver.getIterationCount() < sim.admm_constraint_solver_settings.max_iter);\n BOOST_CHECK(sim.pgs_constraint_solver.getIterationCount() == 0); // make sure pgs didnt run\n q = sim.qnew;\n v = sim.vnew;\n }\n }\n\n {\n Simulator sim(model_handle, geom_model_handle);\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = v0;\n for (size_t i = 0; i < 100; ++i)\n {\n BOOST_CHECK_NO_THROW(sim.step(q, v, zero_torque, dt));\n BOOST_CHECK(sim.pgs_constraint_solver.getIterationCount() < sim.pgs_constraint_solver_settings.max_iter);\n BOOST_CHECK(sim.admm_constraint_solver.getIterationCount() == 0); // make sure admm didnt run\n q = sim.qnew;\n v = sim.vnew;\n }\n }\n}\n\nBOOST_AUTO_TEST_SUITE_END()\n" }, { "path": "tests/forward/simulation-combine-constraints.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \"tests/test_data/config.h\"\n#include \"../test-utils.hpp\"\n\nusing namespace simple;\nusing namespace pinocchio;\nusing ModelHandle = Simulator::ModelHandle;\nusing DataHandle = Simulator::DataHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\nusing GeometryDataHandle = Simulator::GeometryDataHandle;\nusing BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel;\nusing BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector;\nusing WeldConstraintModel = Simulator::WeldConstraintModel;\nusing WeldConstraintModelVector = Simulator::WeldConstraintModelVector;\n\n#define ADMM ::pinocchio::ADMMContactSolverTpl\n\nusing ModelHandle = Simulator::ModelHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\n\n/// \\brief Creates a scene of 10 balls, all lying on a floor.\n/// \\param reduced_collision_pairs activate only the collision pair between balls and floor.\nstd::tuple\ncreateBallsScene(bool prismatic_joint, bool collision_plane, bool inter_colision)\n{\n Model model;\n GeometryModel geom_model;\n\n // Add plane\n hpp::fcl::CollisionGeometryPtr_t plane_ptr(new hpp::fcl::Halfspace(0., 0., 1., 0.));\n const GeometryObject plane_geom = GeometryObject(\"plane\", 0, SE3::Identity(), plane_ptr);\n GeomIndex plane_id = geom_model.addGeometryObject(plane_geom);\n\n // Add balls\n const std::size_t nballs = 10;\n const double radius = 1.0;\n const double mass = 1.0;\n for (std::size_t i = 0; i < nballs; ++i)\n {\n const std::string name = \"ball_\" + std::to_string(i);\n const std::string joint_name = \"joint_\" + std::to_string(i);\n const std::string frame_name = \"frame_\" + std::to_string(i);\n const Inertia inertia = Inertia::FromSphere(mass, radius);\n const JointIndex parent = 0;\n JointIndex joint_id;\n SE3 placement = SE3::Identity();\n ;\n if (prismatic_joint)\n {\n placement.translation() = Eigen::Vector3d(static_cast(i) * 2 * radius * 1.1, 0, 0);\n const JointModelPZ joint = JointModelPZ();\n joint_id = model.addJoint(parent, joint, placement, joint_name);\n }\n else\n {\n const JointModelFreeFlyer joint = JointModelFreeFlyer();\n joint_id = model.addJoint(parent, joint, placement, joint_name);\n }\n model.appendBodyToJoint(joint_id, inertia);\n hpp::fcl::CollisionGeometryPtr_t ball_ptr(new hpp::fcl::Sphere(radius));\n const GeometryObject ball_geom = GeometryObject(name, joint_id, placement, ball_ptr);\n GeomIndex ball_id = geom_model.addGeometryObject(ball_geom);\n if (collision_plane)\n {\n CollisionPair col_pair(plane_id, ball_id);\n geom_model.addCollisionPair(col_pair);\n }\n }\n\n if (inter_colision)\n {\n for (size_t i = 0; i < geom_model.geometryObjects.size(); ++i)\n {\n for (size_t j = i + 1; j < geom_model.geometryObjects.size(); ++j)\n {\n CollisionPair col_pair(i, j);\n geom_model.addCollisionPair(col_pair);\n }\n }\n }\n\n Eigen::VectorXd q0 = neutral(model);\n for (int i = 0; i < (int)(nballs); ++i)\n {\n int nq = prismatic_joint ? 1 : 7;\n if (prismatic_joint)\n {\n q0(i * nq) = radius;\n }\n else\n {\n q0(i * nq) = static_cast(i) * 2 * radius * 1.1;\n q0(i * nq + 2) = radius;\n }\n }\n\n return {\n std::make_shared(model),\n std::make_shared(geom_model),\n q0,\n };\n}\n\nvoid compareConstraintsProblems(const ConstraintsProblem & problem1, const ConstraintsProblem & problem2, const double tol)\n{\n const std::size_t num_contacts = problem1.getNumberOfContacts();\n INDEX_EQUALITY_CHECK(num_contacts, problem2.getNumberOfContacts());\n if (num_contacts != problem2.getNumberOfContacts())\n {\n PINOCCHIO_THROW_PRETTY(std::runtime_error, \"Cannot compare contact problems; They don't have the same amount of contacts.\");\n }\n EIGEN_VECTOR_IS_APPROX(problem1.g(), problem2.g(), tol);\n // Need to compare the constraints cholesky up to `tol`\n // BOOST_CHECK(problem1.constraint_cholesky_decomposition == problem2.constraint_cholesky_decomposition);\n EIGEN_VECTOR_IS_APPROX(problem1.frictional_point_constraints_forces(), problem2.frictional_point_constraints_forces(), tol);\n for (std::size_t i = 0; i < num_contacts; ++i)\n {\n const Simulator::ConstraintModel & cmodel1 = problem1.constraint_models[i];\n const Simulator::ConstraintModel & cmodel2 = problem2.constraint_models[i];\n BOOST_CHECK(cmodel1 == cmodel2);\n BOOST_CHECK(problem1.previous_colliding_collision_pairs[i] == problem2.previous_colliding_collision_pairs[i]);\n }\n}\n\ndouble rand_interval(double rmin, double rmax)\n{\n double val = rand() / (static_cast(RAND_MAX) + 1);\n return (val * (rmax - rmin) + rmin);\n}\n\n// ---------------------------------------------------------------------------------------------------\n// Actual tests\n// ---------------------------------------------------------------------------------------------------\nBOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)\n\n// ---------------------------------------------------------------------------------------------------\n// ----- Test stating scene\n// Given an initial config that should be static test that the system is not moving.\nvoid test_static_scene(bool prismatic_joint)\n{\n // Get system with vertical prismatic, collision with floor and no inter collision\n std::tuple system = createBallsScene(prismatic_joint, true, false);\n ModelHandle model = std::get<0>(system);\n GeometryModelHandle geom_model = std::get<1>(system);\n const Eigen::VectorXd & q0 = std::get<2>(system);\n\n // Initial state\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n\n // ... with this initial state, the balls should not move\n Simulator sim(model, geom_model);\n sim.admm_constraint_solver_settings.absolute_precision = 1e-12;\n sim.admm_constraint_solver_settings.relative_precision = 1e-12;\n const double dt = 1e-3;\n sim.step(q, v, tau, dt);\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), (model->njoints - 1) * (prismatic_joint ? 1 : 6));\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n const std::size_t num_contacts = sim.constraints_problem().getNumberOfContacts();\n INDEX_EQUALITY_CHECK(num_contacts, sim.geom_model().collisionPairs.size());\n BOOST_CHECK(sim.constraints_problem().check());\n BOOST_CHECK(sim.checkCollisionPairs());\n INDEX_EQUALITY_CHECK(sim.constraints_problem().g().size(), static_cast(3 * num_contacts));\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-6);\n EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-6);\n\n // Calling a second time step to test warmstart - contact solver should immediatly converge\n sim.step(q, v, tau, dt);\n // TODO: TEST - It gives 2 instead of 0 :( Warmstart must be solved\n INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 2);\n\n // Running for 5 timesteps\n const std::size_t horizon = 5;\n q = q0;\n v.setZero();\n for (std::size_t i = 0; i < horizon; ++i)\n {\n sim.step(q, v, tau, dt);\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n INDEX_EQUALITY_CHECK(sim.geom_model().collisionPairs.size(), sim.constraints_problem().constraint_models.size());\n const std::size_t num_contacts = sim.constraints_problem().getNumberOfContacts();\n INDEX_EQUALITY_CHECK(sim.geom_model().collisionPairs.size(), num_contacts);\n BOOST_CHECK(sim.constraints_problem().check());\n INDEX_EQUALITY_CHECK(sim.constraints_problem().g().size(), static_cast(3 * num_contacts));\n REAL_IS_APPROX(sim.vnew.norm(), 0.0, 1e-7);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-8);\n EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-8);\n ConstraintsProblem problem1 = sim.constraints_problem();\n\n // Calling a second time step to test warmstart - contact solver should immediatly converge\n sim.step(q, v, tau, dt);\n // TODO: TEST - It gives 3 instead of 0 :( Warmstart must be solved\n INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 3);\n INDEX_EQUALITY_CHECK(sim.constraints_problem().getNumberOfContacts(), num_contacts);\n ConstraintsProblem problem2 = sim.constraints_problem();\n compareConstraintsProblems(problem1, problem2, 1e-5);\n\n // Update state\n q = sim.qnew;\n v = sim.vnew;\n }\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_static_balls_using_joint)\n{\n test_static_scene(true);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_static_balls_using_freeflyer)\n{\n test_static_scene(false);\n}\n\n// ---------------------------------------------------------------------------------------------------\n// ----- Test moving scene\n// Test if the simuation gives the same trajectory with two different simulator constructed differently\n// and eventually with one using warmstart and other one do not\nvoid test_moving_scene(bool prismatic_joint, bool compare_reset, bool inter_collision)\n{\n // Get system\n std::tuple system = createBallsScene(prismatic_joint, true, inter_collision);\n ModelHandle model = std::get<0>(system);\n GeometryModelHandle geom_model = std::get<1>(system);\n Eigen::VectorXd q0 = std::get<2>(system);\n Data _data(*model);\n std::shared_ptr data = std::make_shared(_data);\n GeometryData _geom_data(*geom_model);\n std::shared_ptr geom_data = std::make_shared(_geom_data);\n\n // Initial state\n const std::size_t nballs = geom_model->geometryObjects.size() - 1;\n for (Eigen::Index i = 0; i < static_cast(nballs); ++i)\n {\n Eigen::Index idx(prismatic_joint ? i : i * 7 + 2);\n q0(idx) += rand_interval(0.0, 1.0);\n }\n Eigen::VectorXd v0 = Eigen::VectorXd::Random(model->nv);\n\n std::array sims{Simulator(model, geom_model), Simulator(model, data, geom_model, geom_data)};\n\n // Running for 5000 timesteps, twice\n const std::size_t horizon = 5;\n const double dt = 1e-3;\n std::array, 2> qs;\n std::array, 2> vs;\n std::array, 2> vfrees;\n std::array, 2> constraints_problems;\n std::array, 2> numits;\n for (std::size_t i = 0; i < qs.size(); ++i)\n {\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = v0;\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n qs[i].reserve(horizon);\n vs[i].reserve(horizon);\n vfrees[i].reserve(horizon);\n constraints_problems[i].reserve(horizon);\n numits[i].reserve(horizon);\n\n Simulator & sim = sims[i];\n if (compare_reset && i == 1)\n {\n sim.reset();\n }\n\n for (std::size_t t = 0; t < horizon; ++t)\n {\n sim.step(q, v, tau, dt);\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n BOOST_CHECK(sim.constraints_problem().check());\n\n // Update state\n q = sim.qnew;\n v = sim.vnew;\n\n qs[i].emplace_back(q);\n vs[i].emplace_back(v);\n vfrees[i].emplace_back(sim.vfree);\n constraints_problems[i].emplace_back(sim.constraints_problem());\n numits[i].emplace_back(sim.admm_constraint_solver.getIterationCount());\n }\n }\n\n // Check that trajectories are identical\n for (std::size_t i = 0; i < horizon; ++i)\n {\n const double tol = 1e-10;\n EIGEN_VECTOR_IS_APPROX(qs[0][i], qs[1][i], tol);\n EIGEN_VECTOR_IS_APPROX(vs[0][i], vs[1][i], tol);\n EIGEN_VECTOR_IS_APPROX(vfrees[0][i], vfrees[1][i], tol);\n compareConstraintsProblems(constraints_problems[0][i], constraints_problems[1][i], tol);\n INDEX_EQUALITY_CHECK(numits[0][i], numits[1][i]);\n }\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_joint)\n{\n test_moving_scene(true, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_joint_compare_warmstart)\n{\n test_moving_scene(true, true, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer)\n{\n test_moving_scene(false, false, true);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer_compare_warmstart)\n{\n test_moving_scene(false, true, true);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer_nointercollision)\n{\n test_moving_scene(false, false, false);\n}\n\n// ---------------------------------------------------------------------------------------------------\n// ----- Test one step with different combination of constraints\n// Test if one step give the expected result and the right constraint problem size\nvoid test_constraint_scene(bool prismatic_joint, bool floor_contact, bool bilat, bool weld, bool friction, bool limit, bool rand)\n{\n // Get system\n std::tuple system = createBallsScene(prismatic_joint, floor_contact, false);\n ModelHandle model = std::get<0>(system);\n GeometryModelHandle geom_model = std::get<1>(system);\n Eigen::VectorXd & q0 = std::get<2>(system);\n const std::size_t nballs = (std::size_t)(model->njoints - 1);\n DataHandle data = std::make_shared(*model);\n\n // Add joint constraints\n // Quantities for joint constraints\n // TODO: TEST - Update when nq>1 is available\n bool really_friction = friction && prismatic_joint;\n bool really_limit = limit && prismatic_joint;\n std::size_t n_active_limits = 0;\n const std::size_t n_constrained_v = nballs;\n if (really_friction)\n {\n model->lowerDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * -1.0;\n model->upperDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * 1.0;\n }\n if (really_limit)\n {\n model->lowerPositionLimit = Eigen::VectorXd::Ones(model->nv) * -100000.0;\n model->upperPositionLimit = Eigen::VectorXd::Ones(model->nv) * 100000.0;\n }\n\n // Initial state\n Eigen::VectorXd q = q0;\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n if (rand)\n {\n for (Eigen::Index i = 0; i < static_cast(nballs); ++i)\n {\n Eigen::Index idx(prismatic_joint ? i : i * 7 + 2);\n q[idx] += rand_interval(0.0, 1.0);\n if (q[idx] - model->positionLimitMargin[idx] <= model->lowerPositionLimit(idx))\n {\n n_active_limits++;\n }\n if (q[idx] + model->positionLimitMargin[idx] >= model->upperPositionLimit(idx))\n {\n n_active_limits++;\n }\n }\n v = Eigen::VectorXd::Random(model->nv);\n }\n\n // Adding one attach constraint for each ball if required\n pinocchio::forwardKinematics(*model, *data, q);\n WeldConstraintModelVector weld_constraint_models;\n BilateralPointConstraintModelVector bilateral_constraint_models;\n std::size_t end_bilat = 0;\n std::size_t start_weld = nballs;\n std::size_t attach_constraint_size = 0;\n if (bilat && weld)\n {\n // Half of balls are welded, other are point attached\n end_bilat = nballs / 2;\n start_weld = nballs / 2;\n attach_constraint_size = (nballs / 2) * 3 + (nballs - nballs / 2) * 6;\n }\n else if (bilat && !weld)\n {\n attach_constraint_size = nballs * 3;\n end_bilat = nballs;\n }\n else if (!bilat && weld)\n {\n attach_constraint_size = nballs * 6;\n start_weld = 0;\n }\n else if (!bilat && !weld)\n {\n attach_constraint_size = 0;\n }\n for (std::size_t i = 0; i < end_bilat; ++i)\n {\n const std::string name = \"joint_\" + std::to_string(i);\n const JointIndex joint1_id = model->getJointId(name);\n const GeomIndex joint2_id = 0;\n const SE3 joint1_placement = SE3::Identity();\n const SE3 joint2_placement = data->oMi[joint1_id];\n bilateral_constraint_models.push_back(BilateralPointConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement));\n }\n for (std::size_t i = start_weld; i < nballs; ++i)\n {\n const std::string name = \"joint_\" + std::to_string(i);\n const JointIndex joint1_id = model->getJointId(name);\n const GeomIndex joint2_id = 0;\n const SE3 joint1_placement = SE3::Identity();\n const SE3 joint2_placement = data->oMi[joint1_id];\n weld_constraint_models.push_back(WeldConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement));\n }\n\n // Creating the simulator and make a step\n Simulator sim(model, geom_model, bilateral_constraint_models, weld_constraint_models);\n const double dt = 1e-3;\n sim.step(q, v, tau, dt);\n\n // Test that if balls are on the floor there is 1 contact by ball\n bool is_laying = !rand && floor_contact;\n std::size_t ncontacts = 0;\n if (is_laying)\n {\n ncontacts = nballs;\n INDEX_EQUALITY_CHECK(ncontacts, sim.constraints_problem().getNumberOfContacts());\n }\n\n // Test if that ball should not move the new value are 0\n bool must_not_move = is_laying || (!rand && bilat) || (weld && !bilat);\n if (must_not_move)\n {\n // TODO: TEST - Here precision of 1e-5 is required.. Maybe lack of precision ?\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-5);\n EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-5);\n if (!rand)\n {\n EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-5);\n }\n }\n\n // Test the constraint problem size\n INDEX_EQUALITY_CHECK(\n sim.constraints_problem().constraints_problem_size(),\n (int)(3 * ncontacts + attach_constraint_size + really_friction * n_constrained_v + really_limit * n_active_limits));\n\n // calling the simulator again should not change the state\n // and the solver should converge in one iteration\n if (must_not_move && !rand)\n {\n sim.step(q, v, tau, dt);\n // TODO: TEST - It gives many different results instead of 0 :( Warmstart must be solved\n // Uperbound bound 20 is required\n INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 20);\n }\n}\n\n// test_constraint_scene(\n// prismatic_joint,\n// floor_contact,\n// bilat,\n// weld,\n// friction,\n// limit,\n// rand\n// )\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_no_constraint)\n{\n test_constraint_scene(false, true, false, false, false, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_bilateral_constraint)\n{\n test_constraint_scene(false, false, true, false, false, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_constraint)\n{\n test_constraint_scene(false, false, false, true, false, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_constraint_ill_defined)\n{\n test_constraint_scene(false, false, false, true, false, false, true);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_and_bilat_constraint)\n{\n test_constraint_scene(false, false, true, true, false, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_limit)\n{\n test_constraint_scene(true, false, false, false, false, true, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction)\n{\n test_constraint_scene(true, false, false, false, true, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_limit_static)\n{\n test_constraint_scene(true, true, false, false, false, true, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_static)\n{\n test_constraint_scene(true, true, false, false, true, false, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_joints_constraint_static)\n{\n test_constraint_scene(true, true, false, false, true, true, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints)\n{\n test_constraint_scene(true, false, true, true, true, true, true);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_static)\n{\n test_constraint_scene(true, false, true, true, true, true, false);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_and_contact)\n{\n test_constraint_scene(true, true, true, true, true, true, true);\n}\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_and_contact_static)\n{\n test_constraint_scene(true, true, true, true, true, true, false);\n}\n// Loop on all combination\nBOOST_AUTO_TEST_CASE(simulator_instance_step_test_all_combination)\n{\n for (std::size_t i = 0; i < 128; ++i)\n {\n bool prismatic_joint = i & 0x01;\n bool floor_contact = (i >> 1) & 0x01;\n bool bilat = (i >> 2) & 0x01;\n bool weld = (i >> 3) & 0x01;\n bool friction = (i >> 4) & 0x01;\n bool limit = (i >> 5) & 0x01;\n bool rand = (i >> 6) & 0x01;\n test_constraint_scene(prismatic_joint, floor_contact, bilat, weld, friction, limit, rand);\n }\n}\n\nBOOST_AUTO_TEST_SUITE_END()\n" }, { "path": "tests/forward/simulation-robots.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \"tests/test_data/config.h\"\n#include \"../test-utils.hpp\"\n\nusing namespace simple;\nusing namespace pinocchio;\nusing ModelHandle = Simulator::ModelHandle;\nusing DataHandle = Simulator::DataHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\nusing GeometryDataHandle = Simulator::GeometryDataHandle;\nusing BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel;\nusing BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector;\nusing WeldConstraintModel = Simulator::WeldConstraintModel;\nusing WeldConstraintModelVector = Simulator::WeldConstraintModelVector;\n\n#define ADMM ::pinocchio::ADMMContactSolverTpl\n\nusing ModelHandle = Simulator::ModelHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\n\nBOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)\n\nBOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint)\n{\n // Construct humanoid\n ModelHandle model(new Model());\n buildModels::humanoid(*model, true);\n model->lowerPositionLimit = Eigen::VectorXd::Constant(model->nq, -1.0) * std::numeric_limits::max();\n model->upperPositionLimit = Eigen::VectorXd::Constant(model->nq, 1.0) * std::numeric_limits::max();\n DataHandle data = std::make_shared(*model);\n\n GeometryModelHandle geom_model(new GeometryModel());\n\n // Initial state\n Eigen::VectorXd q = pinocchio::neutral(*model);\n\n // Bilateral constraint on the robot right wrist\n BilateralPointConstraintModelVector bilateral_constraint_models;\n pinocchio::framesForwardKinematics(*model, *data, q);\n const JointIndex joint1_id = 0;\n const GeomIndex joint2_id = 13;\n ::pinocchio::SE3 Mc = data->oMi[joint2_id];\n const SE3 joint1_placement = Mc;\n const SE3 joint2_placement = SE3::Identity();\n bilateral_constraint_models.push_back(BilateralPointConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement));\n bilateral_constraint_models[0].baumgarte_corrector_parameters().Kp = 0.1;\n\n // The humanoid's freeflyer's height should remain the same\n Simulator sim(model, geom_model, bilateral_constraint_models);\n sim.admm_constraint_solver_settings.absolute_precision = 1e-9;\n sim.admm_constraint_solver_settings.relative_precision = 1e-9;\n sim.admm_constraint_solver_settings.max_iter = 1000;\n const double dt = 1e-3;\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt));\n pinocchio::framesForwardKinematics(*model, *data, sim.qnew);\n EIGEN_VECTOR_IS_APPROX(Mc.translation(), data->oMi[joint2_id].translation(), 1e-3);\n\n // Calling the simulator twice to test warmstart\n sim.step(q, v, tau, dt);\n INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0);\n\n for (int i = 0; i < 10; ++i)\n {\n Eigen::VectorXd q = sim.qnew;\n Eigen::VectorXd v = sim.vnew;\n BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt));\n pinocchio::framesForwardKinematics(*model, *data, sim.qnew);\n EIGEN_VECTOR_IS_APPROX(Mc.translation(), data->oMi[joint2_id].translation(), 1e-3);\n }\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_on_joints)\n{\n ModelHandle model(new Model());\n buildModels::manipulator(*model);\n model->lowerDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * -100000.0;\n model->upperDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * 100000.0;\n\n GeometryModelHandle geom_model(new GeometryModel());\n buildModels::manipulatorGeometries(*model, *geom_model);\n\n Simulator sim(model, geom_model);\n\n Eigen::VectorXd q = neutral(*model);\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n const double dt = 1e-3;\n sim.step(q, v, tau, dt);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n // calling the simulator again should not change the state\n // and the solver should converge in one iteration\n sim.step(q, v, tau, dt);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-6);\n EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-6);\n INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0);\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_step_with_limits_on_joints_for_manipulator)\n{\n ModelHandle model(new Model());\n buildModels::manipulator(*model);\n // We first consider not active limits\n model->lowerPositionLimit = Eigen::VectorXd::Ones(model->nq) * -100000.0;\n model->upperPositionLimit = Eigen::VectorXd::Ones(model->nq) * 100000.0;\n\n GeometryModelHandle geom_model(new GeometryModel());\n buildModels::manipulatorGeometries(*model, *geom_model);\n\n Simulator sim(model, geom_model);\n\n Eigen::VectorXd q = neutral(*model);\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n const double dt = 1e-3;\n sim.step(q, v, tau, dt);\n // simulated velocity should be the same as the one computed with aba as constraints are not active\n Eigen::VectorXd vnew = v + dt * pinocchio::aba(*model, sim.data(), q, v, tau);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, vnew, 1e-6);\n // calling the simulator again should not change the state\n // and the solver should converge in one iteration\n sim.step(q, v, tau, dt);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, vnew, 1e-6);\n INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0);\n\n // we now consider active limits\n q = Eigen::VectorXd::Zero(model->nv);\n model->lowerPositionLimit = Eigen::VectorXd::Zero(model->nv);\n model->upperPositionLimit = Eigen::VectorXd::Zero(model->nv);\n\n sim = Simulator(model, geom_model);\n sim.admm_constraint_solver_settings.absolute_precision = 1e-9;\n sim.admm_constraint_solver_settings.relative_precision = 1e-9;\n sim.step(q, v, tau, dt);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n // calling the simulator again should not change the state\n // and the solver should converge in one iteration\n sim.step(q, v, tau, dt);\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0);\n}\n\nBOOST_AUTO_TEST_SUITE_END()\n" }, { "path": "tests/forward/simulator-minimal.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \"tests/test_data/config.h\"\n#include \"../test-utils.hpp\"\n\nusing namespace simple;\nusing namespace pinocchio;\nusing BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel;\nusing BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector;\nusing WeldConstraintModel = Simulator::WeldConstraintModel;\nusing WeldConstraintModelVector = Simulator::WeldConstraintModelVector;\n\nBOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)\n\nBOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint_minimal)\n{\n // Construct humanoid\n Model model;\n buildModels::humanoid(model, true);\n Data data(model);\n\n // Initial state\n Eigen::VectorXd q = pinocchio::neutral(model);\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);\n const double dt = 1e-3;\n\n // Compute vfree and crba for G and g\n crba(model, data, q, Convention::WORLD);\n const Eigen::VectorXd v_free = dt * aba(model, data, q, v, tau, Convention::WORLD);\n\n // Bilateral constraint on the robot right wrist\n pinocchio::forwardKinematics(model, data, q);\n const JointIndex joint1_id = 0;\n const GeomIndex joint2_id = 14;\n assert((int)joint2_id < model.njoints);\n assert(model.nvs[joint2_id] == 1); // make sure its a bilaterable joint\n ::pinocchio::SE3 Mc = data.oMi[joint2_id];\n const SE3 joint1_placement = Mc;\n const SE3 joint2_placement = SE3::Identity();\n\n using ConstraintModel = BilateralPointConstraintModel;\n using ConstraintData = typename ConstraintModel::ConstraintData;\n std::vector constraint_models;\n std::vector constraint_datas;\n ConstraintModel cm(model, joint1_id, joint1_placement, joint2_id, joint2_placement);\n constraint_models.push_back(cm);\n constraint_datas.push_back(cm.createData());\n const Eigen::DenseIndex constraint_size = getTotalConstraintSize(constraint_models);\n\n // Delassus\n ContactCholeskyDecomposition chol(model, constraint_models);\n chol.compute(model, data, constraint_models, constraint_datas, 1e-10);\n\n // Solve constraint\n const Eigen::MatrixXd delassus = chol.getDelassusCholeskyExpression().matrix();\n const DelassusOperatorDense G(delassus);\n\n Eigen::MatrixXd constraint_jacobian(delassus.rows(), model.nv);\n constraint_jacobian.setZero();\n getConstraintsJacobian(model, data, constraint_models, constraint_datas, constraint_jacobian);\n\n const Eigen::VectorXd g = constraint_jacobian * v_free;\n\n PGSContactSolver pgs_solver(int(delassus.rows()));\n pgs_solver.setAbsolutePrecision(1e-10);\n pgs_solver.setRelativePrecision(1e-14);\n pgs_solver.setMaxIterations(1000);\n Eigen::VectorXd primal_solution = Eigen::VectorXd::Zero(constraint_size);\n const bool has_converged =\n pgs_solver.solve(G, g, constraint_models, dt, boost::make_optional((Eigen::Ref)primal_solution));\n BOOST_CHECK(has_converged);\n primal_solution = pgs_solver.getPrimalSolution();\n\n const Eigen::VectorXd tau_ext = constraint_jacobian.transpose() * primal_solution / dt;\n Eigen::VectorXd v_next = v + dt * aba(model, data, q, v, (tau + tau_ext).eval(), Convention::WORLD);\n Eigen::VectorXd dv = v * dt;\n Eigen::VectorXd q_next = ::pinocchio::integrate(model, q, dv);\n\n Eigen::VectorXd v_wrist_expected = Eigen::VectorXd::Zero(model.nvs[joint2_id]);\n\n EIGEN_VECTOR_IS_APPROX(\n v_next.segment(model.idx_vs[joint2_id], model.nvs[joint2_id]), //\n v_wrist_expected, //\n 1e-6);\n pinocchio::forwardKinematics(model, data, q_next);\n EIGEN_VECTOR_IS_APPROX(Mc.translation(), data.oMi[joint2_id].translation(), 1e-6);\n}\n\nBOOST_AUTO_TEST_CASE(humanoid_with_weld_constraint_minimal)\n{\n // Construct humanoid\n Model model;\n buildModels::humanoid(model, true);\n Data data(model);\n\n // Initial state\n Eigen::VectorXd q = pinocchio::neutral(model);\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);\n const double dt = 1e-3;\n\n // Compute vfree and crba for G and g\n crba(model, data, q, Convention::WORLD);\n const Eigen::VectorXd v_free = dt * aba(model, data, q, v, tau, Convention::WORLD);\n\n // Bilateral constraint on the robot right wrist\n pinocchio::forwardKinematics(model, data, q);\n const JointIndex joint1_id = 0;\n const GeomIndex joint2_id = 14;\n assert((int)joint2_id < model.njoints);\n assert(model.nvs[joint2_id] == 1); // make sure its a bilaterable joint\n ::pinocchio::SE3 Mc = data.oMi[joint2_id];\n const SE3 joint1_placement = Mc;\n const SE3 joint2_placement = SE3::Identity();\n\n using ConstraintModel = BilateralPointConstraintModel;\n using ConstraintData = typename ConstraintModel::ConstraintData;\n std::vector constraint_models;\n std::vector constraint_datas;\n ConstraintModel cm(model, joint1_id, joint1_placement, joint2_id, joint2_placement);\n constraint_models.push_back(cm);\n constraint_datas.push_back(cm.createData());\n const Eigen::DenseIndex constraint_size = getTotalConstraintSize(constraint_models);\n\n // Delassus\n ContactCholeskyDecomposition chol(model, constraint_models);\n chol.compute(model, data, constraint_models, constraint_datas, 1e-10);\n\n // Solve constraint\n const Eigen::MatrixXd delassus = chol.getDelassusCholeskyExpression().matrix();\n const DelassusOperatorDense G(delassus);\n\n Eigen::MatrixXd constraint_jacobian(delassus.rows(), model.nv);\n constraint_jacobian.setZero();\n getConstraintsJacobian(model, data, constraint_models, constraint_datas, constraint_jacobian);\n\n const Eigen::VectorXd g = constraint_jacobian * v_free;\n\n PGSContactSolver pgs_solver(int(delassus.rows()));\n pgs_solver.setAbsolutePrecision(1e-10);\n pgs_solver.setRelativePrecision(1e-14);\n pgs_solver.setMaxIterations(1000);\n Eigen::VectorXd primal_solution = Eigen::VectorXd::Zero(constraint_size);\n const bool has_converged =\n pgs_solver.solve(G, g, constraint_models, dt, boost::make_optional((Eigen::Ref)primal_solution));\n BOOST_CHECK(has_converged);\n primal_solution = pgs_solver.getPrimalSolution();\n\n const Eigen::VectorXd tau_ext = constraint_jacobian.transpose() * primal_solution / dt;\n Eigen::VectorXd v_next = v + dt * aba(model, data, q, v, (tau + tau_ext).eval(), Convention::WORLD);\n Eigen::VectorXd dv = v * dt;\n Eigen::VectorXd q_next = ::pinocchio::integrate(model, q, dv);\n\n Eigen::VectorXd v_wrist_expected = Eigen::VectorXd::Zero(model.nvs[joint2_id]);\n\n EIGEN_VECTOR_IS_APPROX(\n v_next.segment(model.idx_vs[joint2_id], model.nvs[joint2_id]), //\n v_wrist_expected, //\n 1e-6);\n pinocchio::forwardKinematics(model, data, q_next);\n EIGEN_VECTOR_IS_APPROX(Mc.translation(), data.oMi[joint2_id].translation(), 1e-6);\n}\n\nBOOST_AUTO_TEST_SUITE_END()\n" }, { "path": "tests/forward/simulator.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \"tests/test_data/config.h\"\n#include \"../test-utils.hpp\"\n\nusing namespace simple;\nusing namespace pinocchio;\nusing ModelHandle = Simulator::ModelHandle;\nusing DataHandle = Simulator::DataHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\nusing GeometryDataHandle = Simulator::GeometryDataHandle;\nusing BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel;\nusing BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector;\nusing WeldConstraintModel = Simulator::WeldConstraintModel;\nusing WeldConstraintModelVector = Simulator::WeldConstraintModelVector;\n\n#define ADMM ::pinocchio::ADMMContactSolverTpl\n\nusing ModelHandle = Simulator::ModelHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\n\nBOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)\n\nBOOST_AUTO_TEST_CASE(simulator_instance_constructor)\n{\n ModelHandle model(new Model());\n DataHandle data(new Data(*model));\n GeometryModelHandle geom_model(new GeometryModel());\n GeometryDataHandle geom_data(new GeometryData(*geom_model));\n BOOST_CHECK_NO_THROW(Simulator sim(model, data, geom_model, geom_data));\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_constructor_2)\n{\n ModelHandle model(new Model());\n GeometryModelHandle geom_model(new GeometryModel());\n BOOST_CHECK_NO_THROW(Simulator sim(model, geom_model));\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_step)\n{\n ModelHandle model(new Model());\n buildModels::humanoidRandom(*model);\n buildModels::manipulator(*model);\n\n GeometryModelHandle geom_model(new GeometryModel());\n buildModels::manipulatorGeometries(*model, *geom_model);\n\n Simulator sim(model, geom_model);\n\n Eigen::VectorXd q = neutral(*model);\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n const double dt = 1e-3;\n BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt));\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_step_make_shared)\n{\n Model _model;\n buildModels::manipulator(_model);\n ModelHandle model = std::make_shared(_model);\n\n GeometryModel _geom_model;\n buildModels::manipulatorGeometries(_model, _geom_model);\n GeometryModelHandle geom_model = std::make_shared(_geom_model);\n Simulator sim(model, geom_model);\n\n Eigen::VectorXd q = neutral(*model);\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n const double dt = 1e-3;\n BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt));\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_step_cubes)\n{\n const int N_cubes = 10;\n std::vector radius(N_cubes, 1.0);\n Model _model;\n GeometryModel _geom_model;\n hpp::fcl::CollisionGeometryPtr_t plane_ptr(new hpp::fcl::Halfspace(0., 0., 1., 0.));\n const FrameIndex plane_frame = FrameIndex(0);\n const GeometryObject plane_geom = GeometryObject(\"plane\", 0, plane_frame, SE3::Identity(), plane_ptr);\n GeomIndex plane_id = _geom_model.addGeometryObject(plane_geom);\n for (int i = 0; i < N_cubes; ++i)\n {\n const std::string name = \"cube_\" + std::to_string(i);\n const std::string joint_name = \"joint_\" + std::to_string(i);\n const std::string frame_name = \"frame_\" + std::to_string(i);\n const double mass = 1.0;\n const double r = radius[static_cast(i)];\n const Inertia inertia = Inertia::FromBox(mass, r, r, r);\n const JointModelFreeFlyer joint = JointModelFreeFlyer();\n const JointIndex parent = 0;\n const SE3 placement = SE3::Identity();\n JointIndex joint_id = _model.addJoint(parent, joint, placement, joint_name);\n _model.appendBodyToJoint(joint_id, inertia);\n hpp::fcl::CollisionGeometryPtr_t box_ptr(new hpp::fcl::Box(r, r, r));\n const GeometryObject box_geom = GeometryObject(name, joint_id, placement, box_ptr);\n GeomIndex box_id = _geom_model.addGeometryObject(box_geom);\n CollisionPair cp(plane_id, box_id);\n _geom_model.addCollisionPair(cp);\n }\n\n std::shared_ptr model = std::make_shared(_model);\n std::shared_ptr geom_model = std::make_shared(_geom_model);\n Simulator sim(model, geom_model);\n sim.admm_constraint_solver_settings.absolute_precision = 1e-12;\n sim.admm_constraint_solver_settings.relative_precision = 1e-12;\n\n Eigen::VectorXd q = neutral(*model);\n for (int i = 0; i < N_cubes; ++i)\n {\n q(i * 7) = static_cast(i) * radius[static_cast(i)] * 1.1;\n q(i * 7 + 2) = radius[static_cast(i)] / 2.;\n }\n Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv);\n const double dt = 1e-3;\n sim.step(q, v, tau, dt);\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n // 4 contact points (cube-plane)\n INDEX_EQUALITY_CHECK(4 * N_cubes, sim.constraints_problem().constraint_models.size());\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n // Calling a second time step to test warmstart\n q = sim.qnew;\n v = sim.vnew;\n sim.step(q, v, tau, dt);\n // TODO: TEST - It gives 2 instead of 0 :( Warmstart must be solved\n INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 2);\n INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv);\n INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints));\n INDEX_EQUALITY_CHECK(4 * N_cubes, sim.constraints_problem().constraint_models.size());\n EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6);\n}\n\nBOOST_AUTO_TEST_CASE(simulator_instance_checkCollisionPairs)\n{\n ModelHandle model(new Model());\n GeometryModelHandle geom_model(new GeometryModel());\n\n // Add balls\n const double radius = 1.0;\n const double mass = 1.0;\n const std::string name1 = \"ball_1\";\n const std::string name2 = \"ball_1\";\n const std::string joint_name = \"joint_1\";\n const std::string frame_name = \"frame_1\";\n const Inertia inertia = Inertia::FromSphere(mass, radius);\n const JointModelFreeFlyer joint = JointModelFreeFlyer();\n const JointIndex parent = 0;\n const SE3 placement = SE3::Identity();\n JointIndex joint_id = model->addJoint(parent, joint, placement, joint_name);\n model->appendBodyToJoint(joint_id, inertia);\n hpp::fcl::CollisionGeometryPtr_t ball_ptr(new hpp::fcl::Sphere(radius));\n const GeometryObject ball_geom1 = GeometryObject(name1, joint_id, placement, ball_ptr);\n const GeometryObject ball_geom2 = GeometryObject(name2, joint_id, placement, ball_ptr);\n GeomIndex ball_id1 = geom_model->addGeometryObject(ball_geom1);\n GeomIndex ball_id2 = geom_model->addGeometryObject(ball_geom2);\n\n CollisionPair col_pair(ball_id1, ball_id2);\n geom_model->addCollisionPair(col_pair);\n\n Simulator sim(model, geom_model);\n BOOST_CHECK(!sim.checkCollisionPairs());\n}\n\nBOOST_AUTO_TEST_SUITE_END()\n" }, { "path": "tests/forward/urdf-romeo.cpp", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \"tests/test_data/config.h\"\n#include \"../test-utils.hpp\"\n\nusing namespace simple;\nusing namespace pinocchio;\nusing ModelHandle = Simulator::ModelHandle;\nusing DataHandle = Simulator::DataHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\nusing GeometryDataHandle = Simulator::GeometryDataHandle;\nusing BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel;\nusing BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector;\nusing WeldConstraintModel = Simulator::WeldConstraintModel;\nusing WeldConstraintModelVector = Simulator::WeldConstraintModelVector;\n\n#define ADMM ::pinocchio::ADMMContactSolverTpl\n\nusing ModelHandle = Simulator::ModelHandle;\nusing GeometryModelHandle = Simulator::GeometryModelHandle;\n\nBOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)\n\nBOOST_AUTO_TEST_CASE(simulator_instance_romeo_step)\n{\n const std::string urdf_filename = PINOCCHIO_MODEL_DIR + std::string(\"/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf\");\n const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string(\"/example-robot-data/robots/romeo_description/srdf/romeo.srdf\");\n std::vector package_dirs;\n const std::string mesh_dir = PINOCCHIO_MODEL_DIR;\n package_dirs.push_back(mesh_dir);\n\n Model _model;\n pinocchio::urdf::buildModel(urdf_filename, pinocchio::JointModelFreeFlyer(), _model);\n GeometryModel _geom_model;\n pinocchio::urdf::buildGeom(_model, urdf_filename, pinocchio::COLLISION, _geom_model, package_dirs);\n _geom_model.addAllCollisionPairs();\n pinocchio::srdf::removeCollisionPairs(_model, _geom_model, srdf_filename, false);\n\n Data _data(_model);\n GeometryData _geom_data(_geom_model);\n\n pinocchio::srdf::loadReferenceConfigurations(_model, srdf_filename, false);\n Eigen::VectorXd q = _model.referenceConfigurations[\"half_sitting\"];\n Eigen::VectorXd v = Eigen::VectorXd::Zero(_model.nv);\n Eigen::VectorXd tau = Eigen::VectorXd::Zero(_model.nv);\n const double dt = 1e-3;\n\n std::shared_ptr model = std::make_shared(_model);\n std::shared_ptr data = std::make_shared(_data);\n std::shared_ptr geom_model = std::make_shared(_geom_model);\n std::shared_ptr geom_data = std::make_shared(_geom_data);\n Simulator sim(model, data, geom_model, geom_data);\n BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt));\n BOOST_CHECK(sim.vfree.size() == model->nv);\n BOOST_CHECK(sim.vnew.size() == model->nv);\n BOOST_CHECK(sim.ftotal.size() == static_cast(model->njoints));\n}\n\nBOOST_AUTO_TEST_SUITE_END()\n" }, { "path": "tests/python/test_simulator_instance.py", "content": "import pinocchio as pin\nimport simple\nimport numpy as np\nfrom hppfcl import (\n Halfspace,\n Sphere,\n HeightFieldAABB,\n CollisionRequest,\n CollisionResult,\n collide,\n)\n\ntry:\n import example_robot_data as erd\n\n has_erd = True\nexcept:\n print(\"Cannot find example robot data. Skipping simulator tests.\")\n has_erd = False\n\n\ndef setBallsAndHalfSpace(length=[0.2], mass=[1.0]):\n assert len(length) == len(mass) or len(length) == 1 or len(mass) == 1\n N = max(len(length), len(mass))\n if len(length) == 1:\n length = length * N\n if len(mass) == 1:\n mass = mass * N\n model = pin.Model()\n geom_model = pin.GeometryModel()\n\n # create plane for floor\n n = np.array([0.0, 0.0, 1])\n plane_shape = Halfspace(n, 0)\n T = pin.SE3(np.eye(3), np.zeros(3))\n plane = pin.GeometryObject(\"plane\", 0, 0, T, plane_shape)\n plane.meshColor = np.array([0.5, 0.5, 0.5, 1.0])\n plane_id = geom_model.addGeometryObject(plane)\n ball_ids = []\n for n_ball in range(N):\n a = length[n_ball]\n m = mass[n_ball]\n freeflyer = pin.JointModelFreeFlyer()\n joint = model.addJoint(0, freeflyer, pin.SE3.Identity(), \"ball_\" + str(n_ball))\n model.appendBodyToJoint(\n joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity()\n )\n ball_shape = Sphere(a / 2)\n geom_ball = pin.GeometryObject(\n \"ball_\" + str(n_ball), joint, joint, pin.SE3.Identity(), ball_shape\n )\n geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0])\n ball_id = geom_model.addGeometryObject(geom_ball)\n for id in ball_ids:\n col_pair = pin.CollisionPair(id, ball_id)\n geom_model.addCollisionPair(col_pair)\n ball_ids += [ball_id]\n col_pair = pin.CollisionPair(plane_id, ball_id)\n geom_model.addCollisionPair(col_pair)\n\n model.qref = pin.neutral(model)\n model.qinit = model.qref.copy()\n for n_ball in range(N):\n model.qinit[7 * n_ball] += a\n model.qinit[7 * n_ball + 2] += 0.1\n\n data = model.createData()\n geom_data = geom_model.createData()\n for req in geom_data.collisionRequests:\n req.security_margin = 1e-3\n return model, data, geom_model, geom_data\n\n\ndef test_init():\n model = pin.Model()\n geom_model = pin.GeometryModel()\n data = model.createData()\n geom_data = geom_model.createData()\n sim = simple.Simulator(model, data, geom_model, geom_data)\n assert True\n\n\ndef test_init2():\n model = pin.Model()\n geom_model = pin.GeometryModel()\n sim = simple.Simulator(model, geom_model)\n assert True\n\n\ndef test_simulator_handles():\n model = pin.Model()\n geom_model = pin.GeometryModel()\n data = model.createData()\n geom_data = geom_model.createData()\n\n sim = simple.Simulator(model, data, geom_model, geom_data)\n\n model2 = pin.buildSampleModelHumanoid()\n model2.lowerPositionLimit = -np.ones((model2.nq, 1))\n model2.upperPositionLimit = np.ones((model2.nq, 1))\n geom_model2 = pin.buildSampleGeometryModelHumanoid(model2)\n data2 = model2.createData()\n geom_data2 = geom_model2.createData()\n sim = simple.Simulator(model2, data2, geom_model2, geom_data2)\n q = pin.randomConfiguration(model2)\n pin.updateGeometryPlacements(sim.model, sim.data, sim.geom_model, sim.geom_data, q)\n\n assert sim.model == model2\n assert not (sim.model == model)\n assert sim.geom_model == geom_model2\n assert not (sim.geom_model == geom_model)\n assert sim.geom_data == geom_data2\n assert not (sim.geom_data == geom_data)\n\n model3 = model2.copy()\n freeflyer = pin.JointModelFreeFlyer()\n joint = model2.addJoint(0, freeflyer, pin.SE3.Identity(), \"ball_0\")\n assert sim.model.njoints == model3.njoints + 1\n gemo_model3 = geom_model2.copy()\n ball_shape = Sphere(0.1 / 2)\n geom_ball = pin.GeometryObject(\n \"ball_0\", joint, joint, pin.SE3.Identity(), ball_shape\n )\n ball_id = geom_model2.addGeometryObject(geom_ball)\n assert sim.geom_model.ngeoms == gemo_model3.ngeoms + 1\n\n\ndef test_init_empty():\n model = pin.Model()\n geom_model = pin.GeometryModel()\n data = model.createData()\n geom_data = geom_model.createData()\n sim = simple.Simulator(model, data, geom_model, geom_data)\n q = pin.neutral(model)\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n assert True\n\n\ndef test_step():\n model = pin.buildSampleModelManipulator()\n geom_model = pin.buildSampleGeometryModelManipulator(model)\n data = model.createData()\n geom_data = geom_model.createData()\n\n sim = simple.Simulator(model, data, geom_model, geom_data)\n\n q = pin.randomConfiguration(model)\n v = np.random.random(model.nv)\n tau = np.random.random(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n\n fext = [pin.Force(np.random.random(6)) for i in range(model.njoints)]\n sim.step(q, v, tau, fext, dt)\n assert True\n\n\ndef test_step_balls():\n mass = [12.0]\n length = [1.5]\n model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass)\n\n q = pin.neutral(model)\n q[2] = length[0] / 2.0 + 1.0\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim = simple.Simulator(model, data, geom_model, geom_data)\n sim.step(q, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n\n q[2] = 0.0\n v = np.zeros(model.nv)\n sim.step(q, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 1\n assert np.linalg.norm(sim.vnew) < 1e-6\n mass = [12.0, 0.1]\n length = [1.5, 0.5]\n model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass)\n sim = simple.Simulator(model, data, geom_model, geom_data)\n\n q = pin.neutral(model)\n q[0] = 0.0\n q[7] = length[1] / 2.0 + 1.0\n q[2] = 0.0\n q[9] = 0.0\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n sim.step(q, v, tau, dt)\n assert np.linalg.norm(sim.vnew) < 1e-6\n num_contacts = sim.constraints_problem.getNumberOfContacts()\n assert num_contacts == 2\n lam = sim.constraints_problem.frictional_point_constraints_forces()[\n : 3 * num_contacts\n ]\n assert len(lam) == 3 * num_contacts\n sim.step(q, v, tau, dt)\n assert True\n\n\ndef test_step_balls_with_constraints():\n nballs = 2\n mass = [23.0] * nballs\n length = [2.0] * nballs\n model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass)\n model.lowerPositionLimit = np.ones(model.nq) * -100.0\n model.upperPositionLimit = np.ones(model.nq) * 100.0\n\n q0 = pin.neutral(model)\n for i in range(nballs):\n q0[7 * i] = i * length[i] * 1.5\n q0[7 * i + 2] = 10.0\n q = q0.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim = simple.Simulator(model, data, geom_model, geom_data)\n sim.step(q, v, tau, dt)\n vnew = v + dt * pin.aba(model, data, q, v, tau)\n assert sim.constraints_problem.constraints_problem_size() == nballs * 3 * 2\n assert np.linalg.norm(sim.vnew - vnew) < 1e-6\n\n model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass)\n model.lowerPositionLimit = np.ones(model.nq) * -100.0\n model.upperPositionLimit = np.ones(model.nq) * 100.0\n for i in range(nballs // 2):\n model.lowerPositionLimit[7 * i + 2] = 10.0\n q = q0.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim = simple.Simulator(model, data, geom_model, geom_data)\n sim.step(q, v, tau, dt)\n vnew = v + dt * pin.aba(model, data, q, v, tau)\n assert sim.constraints_problem.constraints_problem_size() == nballs * 3 * 2\n assert (\n np.linalg.norm(sim.vnew[(nballs // 2) * 6 :] - vnew[(nballs // 2) * 6 :]) < 1e-6\n )\n assert np.linalg.norm(sim.vnew[: (nballs // 2) * 6]) < 1e-6\n\n\ndef addSystemCollisionPairs(model, geom_model, qref):\n \"\"\"\n Add the right collision pairs of a model, given qref.\n qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision\n in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered\n to always be in collision.\n \"\"\"\n data = model.createData()\n geom_data = geom_model.createData()\n pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref)\n geom_model.removeAllCollisionPairs()\n num_col_pairs = 0\n for i in range(len(geom_model.geometryObjects)):\n for j in range(i + 1, len(geom_model.geometryObjects)):\n # Don't add collision pair if same object\n if i != j:\n gobj_i: pin.GeometryObject = geom_model.geometryObjects[i]\n gobj_j: pin.GeometryObject = geom_model.geometryObjects[j]\n if gobj_i.name == \"floor\" or gobj_j.name == \"floor\":\n num_col_pairs += 1\n col_pair = pin.CollisionPair(i, j)\n geom_model.addCollisionPair(col_pair)\n else:\n if gobj_i.parentJoint != gobj_j.parentJoint:\n # Compute collision between the geometries. Only add the collision pair if there is no collision.\n M1 = geom_data.oMg[i]\n M2 = geom_data.oMg[j]\n colreq = CollisionRequest()\n colreq.security_margin = 1e-2 # 1cm of clearance\n colres = CollisionResult()\n collide(\n gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres\n )\n if not colres.isCollision():\n num_col_pairs += 1\n col_pair = pin.CollisionPair(i, j)\n geom_model.addCollisionPair(col_pair)\n\n\ndef addFloor(geom_model: pin.GeometryModel):\n floor_collision_shape = Halfspace(0, 0, 1, 0)\n M = pin.SE3.Identity()\n floor_collision_object = pin.GeometryObject(\"floor\", 0, 0, M, floor_collision_shape)\n geom_model.addGeometryObject(floor_collision_object)\n\n\ndef test_manipulator_limits():\n model = pin.buildSampleModelManipulator()\n geom_model = pin.buildSampleGeometryModelManipulator(model)\n q0 = pin.neutral(model)\n model.lowerPositionLimit = q0.copy()\n model.upperPositionLimit = q0.copy()\n sim = simple.Simulator(model, geom_model)\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q0, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq\n assert np.linalg.norm(sim.vnew) < 1e-6\n\n\ndef test_humanoid_limits():\n model = pin.buildSampleModelHumanoid()\n geom_model = pin.buildSampleGeometryModelHumanoid(model)\n q0 = pin.neutral(model)\n model.lowerPositionLimit = q0.copy()\n model.upperPositionLimit = q0.copy()\n sim = simple.Simulator(model, geom_model)\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q0, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == 2 * (model.nq - 4)\n assert sim.admm_constraint_solver.getAbsoluteConvergenceResidual() < 1e-6\n\n\ndef test_freeflyer_limits():\n model = pin.Model()\n geom_model = pin.GeometryModel()\n a = 0.1\n m = 3.8\n freeflyer = pin.JointModelFreeFlyer()\n joint = model.addJoint(0, freeflyer, pin.SE3.Identity(), \"ball\")\n model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity())\n ball_shape = Sphere(a / 2)\n geom_ball = pin.GeometryObject(\"ball\", joint, joint, pin.SE3.Identity(), ball_shape)\n geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0])\n ball_id = geom_model.addGeometryObject(geom_ball)\n q0 = pin.neutral(model)\n model.lowerPositionLimit = q0.copy()\n model.upperPositionLimit = q0.copy()\n sim = simple.Simulator(model, geom_model)\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q0, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == 2 * (model.nq - 4)\n assert np.linalg.norm(sim.vnew) < 1e-6\n\n\ndef test_composite_limits():\n model = pin.Model()\n geom_model = pin.GeometryModel()\n a = 0.1\n m = 3.8\n composite = pin.JointModelComposite(2)\n composite.addJoint(pin.JointModelRX())\n composite.addJoint(pin.JointModelRY())\n joint = model.addJoint(0, composite, pin.SE3.Identity(), \"ball\")\n model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity())\n ball_shape = Sphere(a / 2)\n geom_ball = pin.GeometryObject(\"ball\", joint, joint, pin.SE3.Identity(), ball_shape)\n geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0])\n ball_id = geom_model.addGeometryObject(geom_ball)\n q0 = pin.neutral(model)\n model.lowerPositionLimit = q0.copy()\n model.upperPositionLimit = q0.copy()\n sim = simple.Simulator(model, geom_model)\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q0, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq\n assert np.linalg.norm(sim.vnew) < 1e-6\n model = pin.Model()\n geom_model = pin.GeometryModel()\n a = 0.1\n m = 3.8\n composite = pin.JointModelComposite(2)\n composite.addJoint(pin.JointModelPX())\n composite.addJoint(pin.JointModelPY())\n joint = model.addJoint(0, composite, pin.SE3.Identity(), \"ball\")\n model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity())\n ball_shape = Sphere(a / 2)\n geom_ball = pin.GeometryObject(\"ball\", joint, joint, pin.SE3.Identity(), ball_shape)\n geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0])\n ball_id = geom_model.addGeometryObject(geom_ball)\n q0 = pin.neutral(model)\n model.lowerPositionLimit = q0.copy()\n model.upperPositionLimit = q0.copy()\n sim = simple.Simulator(model, geom_model)\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q0, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq\n assert np.linalg.norm(sim.vnew) < 1e-6\n\n\ndef test_mujoco_humanoid_limits():\n import os\n\n path = os.path.abspath(__file__)\n path = path.split(\"/\")\n path = \"/\".join(path[:-2])\n path = os.path.join(path, \"test_data/mujoco_humanoid.xml\")\n model = pin.buildModelFromMJCF(path)\n geom_model = pin.buildGeomFromMJCF(model, path, pin.COLLISION)\n addFloor(geom_model)\n q0 = pin.neutral(model)\n addSystemCollisionPairs(model, geom_model, q0)\n\n sim = simple.Simulator(model, geom_model)\n # First we test without contact but with limits\n q = q0.copy()\n q[2] += 1.0\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n data = model.createData()\n vnew = v + dt * pin.aba(model, data, q, v, tau)\n assert np.linalg.norm(sim.vnew - vnew) < 1e-6\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == (model.nq - 4) * 2\n # Now we test without contact but with tight limits\n model.lowerPositionLimit = q0.copy()\n model.lowerPositionLimit[2] += 1.0\n model.upperPositionLimit = model.lowerPositionLimit.copy()\n sim = simple.Simulator(model, geom_model)\n q = model.lowerPositionLimit.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 0\n assert sim.constraints_problem.constraints_problem_size() == (model.nq - 4) * 2\n assert sim.admm_constraint_solver.getAbsoluteConvergenceResidual() < 1e-6\n # Now we test with contact but without limits\n model.lowerPositionLimit = -np.ones(model.nq) * np.finfo(\"d\").max\n model.upperPositionLimit = np.ones(model.nq) * np.finfo(\"d\").max\n sim = simple.Simulator(model, geom_model)\n q = q0.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n\n assert (\n sim.constraints_problem.constraints_problem_size()\n == sim.constraints_problem.getNumberOfContacts() * 3\n )\n assert sim.admm_constraint_solver.getAbsoluteConvergenceResidual() < 1e-6\n # Now we test with contact and with large limits\n model.lowerPositionLimit = -np.ones(model.nq) * 10000.0\n model.upperPositionLimit = np.ones(model.nq) * 10000.0\n sim = simple.Simulator(model, geom_model)\n q = q0.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n assert (\n sim.constraints_problem.constraints_problem_size()\n == (model.nq - 4) * 2 + 3 * sim.constraints_problem.getNumberOfContacts()\n )\n # TODO Now we test with contact and limits\n model = pin.buildModelFromMJCF(path)\n sim = simple.Simulator(model, geom_model)\n q = q0.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim.step(q, v, tau, dt)\n\n\nif has_erd:\n\n def setSolo():\n # robot model\n robot = erd.load(\"solo12\")\n model = robot.model.copy()\n model.qref = np.array(\n [\n 0.09906518,\n 0.20099078,\n 0.32502457,\n 0.19414175,\n -0.00524735,\n -0.97855773,\n 0.06860185,\n 0.00968163,\n 0.60963582,\n -1.61206407,\n -0.02543309,\n 0.66709088,\n -1.50870083,\n 0.32405118,\n -1.15305599,\n 1.56867351,\n -0.39097222,\n -1.29675892,\n 1.39741073,\n ]\n )\n model.qref = pin.normalize(model, model.qref)\n\n # Create height field ground\n def ground(xy):\n return (\n np.sin(xy[0] * 3) / 5\n + np.cos(xy[1] ** 2 * 3) / 20\n + np.sin(xy[1] * xy[0] * 5) / 10\n )\n\n xg = np.arange(-2, 2, 0.02)\n nx = xg.shape[0]\n xy_g = np.meshgrid(xg, xg)\n xy_g = np.stack(xy_g)\n elev_g = np.zeros((nx, nx))\n elev_g[:, :] = ground(xy_g)\n sx = xg[-1] - xg[0]\n sy = xg[-1] - xg[0]\n elev_g[:, :] = elev_g[::-1, :]\n\n # Create geometry model\n geom_model = robot.collision_model\n hfield = HeightFieldAABB(sx, sy, elev_g, np.min(elev_g))\n Mhfield = pin.SE3.Identity()\n ground = pin.GeometryObject(\"ground\", 0, Mhfield, hfield)\n ground_id = geom_model.addGeometryObject(ground)\n\n # Add collision pairs between the ground and the robot\n geom_model.removeAllCollisionPairs()\n\n a = 0.01910275\n frames_names = [\"HR_FOOT\", \"HL_FOOT\", \"FR_FOOT\", \"FL_FOOT\"]\n\n for name in frames_names:\n frame_id = model.getFrameId(name)\n frame = model.frames[frame_id]\n joint_id = frame.parentJoint\n frame_placement = frame.placement\n\n shape_name = name + \"_shape\"\n shape = Sphere(a)\n geometry = pin.GeometryObject(shape_name, joint_id, frame_placement, shape)\n geometry.meshColor = np.array([1.0, 0.2, 0.2, 1.0])\n\n geom_id = geom_model.addGeometryObject(geometry)\n\n foot_plane = pin.CollisionPair(\n ground_id, geom_id\n ) # order should be inverted ?\n geom_model.addCollisionPair(foot_plane)\n\n # Create data\n data = model.createData()\n geom_data = geom_model.createData()\n for req in geom_data.collisionRequests:\n req.security_margin = 1e-3\n return model, data, geom_model, geom_data\n\n def test_step_solo():\n model, data, geom_model, geom_data = setSolo()\n q = model.qref.copy()\n v = np.zeros(model.nv)\n tau = np.zeros(model.nv)\n dt = 1e-3\n sim = simple.Simulator(model, data, geom_model, geom_data)\n sim.step(q, v, tau, dt)\n assert sim.constraints_problem.getNumberOfContacts() == 4\n\n\n# test_init()\n# test_init2()\n# test_simulator_handles()\n# test_init_empty()\n# test_step()\n# test_step_balls()\n# test_step_balls_with_constraints()\n# test_humanoid_limits()\n# test_freeflyer_limits()\n# test_composite_limits()\n# test_manipulator_limits()\n# test_mujoco_humanoid_limits()\n# print(\"All tests passed\")\n" }, { "path": "tests/test-utils.hpp", "content": "#ifndef __simple__test_utils_hpp__\n#define __simple__test_utils_hpp__\n\n#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \\\n BOOST_CHECK_MESSAGE( \\\n ((Va) - (Vb)).isZero(precision), \"check \" #Va \".isApprox(\" #Vb \") failed at precision \" \\\n << precision << \". (\" #Va \" - \" #Vb \").norm() = \" << ((Va) - (Vb)).norm() << \" [\\n\" \\\n << (Va).transpose() << \"\\n!=\\n\" \\\n << (Vb).transpose() << \"\\n]\")\n\n#define INDEX_EQUALITY_CHECK(i1, i2) BOOST_CHECK_MESSAGE(i1 == i2, \"check \" #i1 \"==\" #i2 \" failed. [\" << i1 << \" != \" << i2 << \"]\")\n#define INDEX_INEQUALITY_CHECK(i1, i2) BOOST_CHECK_MESSAGE(i1 <= i2, \"check \" #i1 \"==\" #i2 \" failed. [\" << i1 << \" != \" << i2 << \"]\")\n\n#define REAL_IS_APPROX(a, b, precision) \\\n BOOST_CHECK_MESSAGE( \\\n std::abs((a) - (b)) < precision, \\\n \"check std::abs(\" #a \" - \" #b \") = \" << std::abs((a) - (b)) << \" < \" << precision << \" failed. [\" << a << \" != \" << b << \"]\")\n\n#endif\n" }, { "path": "tests/test_data/config.h.in", "content": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_test_data_config_h__\n#define __simple_test_data_config_h__\n\n#define SIMPLE_TEST_DATA_DIR \"@PROJECT_SOURCE_DIR@/tests/test_data\"\n\n#endif // __simple_test_data_config_h__\n\n" }, { "path": "tests/test_data/mujoco_humanoid.xml", "content": "\n\n\n \n" } ]