Repository: Simple-Robotics/Simple Branch: main Commit: 03d4784ed87b Files: 81 Total size: 441.5 KB Directory structure: gitextract_kdce3rlq/ ├── .clang-format ├── .cmake-format.yaml ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── benchmark/ │ ├── CMakeLists.txt │ ├── affine-transform.cpp │ └── mujoco-humanoid.cpp ├── bindings/ │ ├── CMakeLists.txt │ └── python/ │ ├── CMakeLists.txt │ ├── core/ │ │ ├── expose-constraints-problem.cpp │ │ ├── expose-contact-frame.cpp │ │ └── expose-simulator.cpp │ ├── module.cpp │ └── simple/ │ └── __init__.py ├── include/ │ └── simple/ │ ├── bindings/ │ │ └── python/ │ │ ├── core/ │ │ │ ├── constraints-problem.hpp │ │ │ └── simulator.hpp │ │ └── fwd.hpp │ ├── core/ │ │ ├── constraints-problem.hpp │ │ ├── constraints-problem.hxx │ │ ├── constraints-problem.txx │ │ ├── contact-frame.hpp │ │ ├── contact-frame.hxx │ │ ├── fwd.hpp │ │ ├── simulator.hpp │ │ ├── simulator.hxx │ │ └── simulator.txx │ ├── fwd.hpp │ ├── math/ │ │ ├── fwd.hpp │ │ └── qr.hpp │ ├── pch.hpp │ ├── pinocchio_template_instantiation/ │ │ ├── aba-derivatives.txx │ │ ├── aba.txx │ │ ├── crba.txx │ │ └── joint-model.txx │ └── utils/ │ └── visitors.hpp ├── sandbox/ │ ├── cartpole.py │ ├── cassie_mj.py │ ├── force_action_derivative.py │ ├── four_bar_linkage.py │ ├── four_five_bar_linkage.py │ ├── go2_contact_id.py │ ├── humanoid_mj.py │ ├── parallel_rollout.py │ ├── pendulum.py │ ├── pin_utils.py │ ├── robots/ │ │ ├── four_bar_linkage.xml │ │ ├── four_five_bar_linkage.xml │ │ ├── go2/ │ │ │ └── mjcf/ │ │ │ ├── go2.xml │ │ │ └── scene.xml │ │ ├── humanoid.xml │ │ └── pendulum.xml │ ├── sim_utils.py │ ├── simulation_args.py │ ├── simulation_utils.py │ ├── test_memory.py │ └── viz_utils.py ├── sources.cmake ├── src/ │ ├── CMakeLists.txt │ ├── core/ │ │ ├── constraints-problem.cpp │ │ └── simulator.cpp │ ├── empty.cpp │ └── pinocchio_template_instantiation/ │ ├── aba-derivatives.cpp │ ├── aba.cpp │ ├── crba.cpp │ └── joint-model.cpp └── tests/ ├── CMakeLists.txt ├── forward/ │ ├── CMakeLists.txt │ ├── mujoco-humanoid.cpp │ ├── simulation-combine-constraints.cpp │ ├── simulation-robots.cpp │ ├── simulator-minimal.cpp │ ├── simulator.cpp │ └── urdf-romeo.cpp ├── python/ │ └── test_simulator_instance.py ├── test-utils.hpp └── test_data/ ├── config.h.in └── mujoco_humanoid.xml ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ Language: Cpp BasedOnStyle: Microsoft AlwaysBreakTemplateDeclarations: Yes BinPackParameters: false BreakBeforeBinaryOperators: NonAssignment BreakConstructorInitializers: BeforeComma BreakInheritanceList: BeforeComma ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 IndentWidth: 2 NamespaceIndentation: All PackConstructorInitializers: Never PenaltyReturnTypeOnItsOwnLine: 10 PointerAlignment: Middle SpaceAfterTemplateKeyword: false ColumnLimit: 140 SortIncludes: false IndentPPDirectives: BeforeHash AlignAfterOpenBracket: AlwaysBreak ================================================ FILE: .cmake-format.yaml ================================================ markup: first_comment_is_literal: true format: line_width: 100 parse: additional_commands: cxx_flags_by_compiler_frontend: pargs: 0 kwargs: OUTPUT: 1 GNU: '*' MSVC: '*' FILTER: 0 modernize_target_link_libraries: pargs: 1 kwargs: SCOPE: 1 TARGETS: '*' LIBRARIES: '*' INCLUDE_DIRS: '*' pinocchio_target: pargs: 1 kwargs: INTERFACE: 0 SCALAR: 1 LIBRARY_PUBLIC_SCOPE: 1 SOURCES: '*' add_pinocchio_cpp_example: pargs: 1 kwargs: PARSERS: 0 CPPAD: 0 CPPADCG: 0 CASADI: 0 add_pinocchio_unit_test: pargs: 1 kwargs: HEADER_ONLY: 0 PARSERS: 0 EXTRA: 0 COLLISION: 0 PARALLEL: 0 PARSERS_OPTIONAL: 0 EXTRA_OPTIONAL: 0 COLLISION_OPTIONAL: 0 PARALLEL_OPTIONAL: 0 PACKAGES: '*' install_python_files: pargs: 0 kwargs: MODULE: 1 FILES: '*' ================================================ FILE: .gitignore ================================================ build* Xcode* *~ *.pyc **/.ipynb_checkpoints coverage* .travis .vscode* *.orig .cache ================================================ FILE: .gitmodules ================================================ [submodule "cmake"] path = cmake url = https://github.com/jrl-umi3218/jrl-cmakemodules.git ================================================ FILE: .pre-commit-config.yaml ================================================ ci: autoupdate_branch: devel autofix_prs: false repos: - repo: https://github.com/pre-commit/mirrors-clang-format rev: v18.1.5 hooks: - id: clang-format types_or: [] types: [text] files: \.(cpp|cxx|c|h|hpp|hxx|txx)$ - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.6.0 hooks: - id: check-added-large-files - id: check-case-conflict - id: check-yaml exclude: ^packaging/conda/ - id: detect-private-key # - id: end-of-file-fixer - id: mixed-line-ending - id: check-merge-conflict - id: trailing-whitespace exclude: | (?x)^( doc/doxygen-awesome.* )$ - repo: https://github.com/astral-sh/ruff-pre-commit rev: v0.4.4 hooks: - id: ruff-format exclude: doc/ - repo: https://github.com/cheshirekow/cmake-format-precommit rev: v0.6.13 hooks: - id: cmake-format additional_dependencies: [pyyaml>=5.1] - repo: https://github.com/Lucas-C/pre-commit-hooks rev: v1.5.5 hooks: - id: forbid-tabs - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt rev: 0.2.3 hooks: - id: yamlfmt args: [--mapping=2, --offset=2, --sequence=4, --implicit_start] ================================================ FILE: CMakeLists.txt ================================================ # # Copyright (c) 2024 INRIA # cmake_minimum_required(VERSION 3.10) set(PROJECT_NAME simple) set(PROJECT_DESCRIPTION "The Simple Simulator: Simulation Made Simple") set(PROJECT_URL "http://github.com/simple-robotics/simple") set(PROJECT_CUSTOM_HEADER_EXTENSION "hpp") set(PROJECT_USE_CMAKE_EXPORT TRUE) set(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE) set(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion) set(SIMPLE_PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}) # Disable -Werror on Unix for now. set(CXX_DISABLE_WERROR True) set(CMAKE_VERBOSE_MAKEFILE True) # ---------------------------------------------------- # --- OPTIONS --------------------------------------- # ---------------------------------------------------- # Need to be set before including base.cmake option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) option(USE_PRECOMPILED_HEADERS "Use pre-compiled headers for pinocchio and eigen." OFF) # Check if the submodule cmake have been initialized set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake") message(STATUS "JRL cmakemodules found in 'cmake/' git submodule") else() find_package(jrl-cmakemodules QUIET CONFIG) if(jrl-cmakemodules_FOUND) get_property( JRL_CMAKE_MODULES TARGET jrl-cmakemodules::jrl-cmakemodules PROPERTY INTERFACE_INCLUDE_DIRECTORIES) message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}") elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0") message( FATAL_ERROR "\nCan't find jrl-cmakemodules. Please either:\n" " - use git submodule: 'git submodule update --init'\n" " - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n" " - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n") else() message(STATUS "JRL cmakemodules not found. Let's fetch it.") include(FetchContent) FetchContent_Declare( "jrl-cmakemodules" GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git" EXCLUDE_FROM_ALL) FetchContent_MakeAvailable("jrl-cmakemodules") FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES) endif() endif() set(DOXYGEN_USE_MATHJAX YES) set(DOXYGEN_USE_TEMPLATE_CSS YES) if(POLICY CMP0177) cmake_policy(SET CMP0177 NEW) set(CMAKE_POLICY_DEFAULT_CMP0177 NEW) endif() include("${JRL_CMAKE_MODULES}/base.cmake") compute_project_args(PROJECT_ARGS LANGUAGES CXX) project(${PROJECT_NAME} ${PROJECT_ARGS}) include("${JRL_CMAKE_MODULES}/tracy.cmake") include("${JRL_CMAKE_MODULES}/boost.cmake") include("${JRL_CMAKE_MODULES}/ide.cmake") include("${JRL_CMAKE_MODULES}/apple.cmake") if(APPLE) # Use the handmade approach if(${CMAKE_VERSION} VERSION_LESS "3.18.0") # Need to find the right version set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake/find-external/OpenMP ${CMAKE_MODULE_PATH}) endif() elseif(UNIX) if(${CMAKE_VERSION} VERSION_EQUAL "3.20.0") set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake/find-external/OpenMP ${CMAKE_MODULE_PATH}) endif() endif() include(CMakeDependentOption) # If needed, set CMake policy for APPLE systems apply_default_apple_configuration() if(CMAKE_VERSION VERSION_GREATER "3.12") cmake_policy(SET CMP0074 NEW) endif() # Force C++ standard to be C++14 at least check_minimal_cxx_standard(14 ENFORCE) option(BUILD_BENCHMARK "Build the benchmarks" OFF) option(BUILD_PYTHON_INTERFACE "Build the Python bindings" ON) option(GENERATE_PYTHON_STUBS "Generate the Python stubs associated to the Python library" OFF) option(BUILD_WITH_COMMIT_VERSION "Build libraries by setting specific commit version" OFF) option(SIMPLE_USE_DIFFCOAL "Use diffcoal to compute collision detection derivatives" OFF) option(BUILD_WITH_OPENMP_SUPPORT "Build the library with the OpenMP support" OFF) if(APPLE) option(BUILD_WITH_ACCELERATE_SUPPORT "Build EigenPy with the Accelerate support" OFF) else(APPLE) set(BUILD_WITH_ACCELERATE_SUPPORT FALSE) endif(APPLE) option(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF) option(CHECK_RUNTIME_MALLOC "Check if some memory allocations are performed at runtime" OFF) option(ENABLE_TEMPLATE_INSTANTIATION "Template instantiation of the main library" ON) option(SIMPLE_DOWNLOAD_TRACY "Use FetchContent to install Tracy." OFF) set(CFLAGS_OPTIONS) if(INITIALIZE_WITH_NAN) message(STATUS "Initialize with NaN all the Eigen entries.") endif(INITIALIZE_WITH_NAN) if(CHECK_RUNTIME_MALLOC) message(STATUS "Check if some memory allocations are performed at runtime.") endif(CHECK_RUNTIME_MALLOC) if(ENABLE_TEMPLATE_INSTANTIATION) message(STATUS "Template instantiation of the main library") list(APPEND CFLAGS_OPTIONS "-DSIMPLE_ENABLE_TEMPLATE_INSTANTIATION") endif(ENABLE_TEMPLATE_INSTANTIATION) macro(TAG_LIBRARY_VERSION target) set_target_properties(${target} PROPERTIES SOVERSION ${PROJECT_VERSION}) endmacro(TAG_LIBRARY_VERSION) # ---------------------------------------------------- # --- DEPENDENCIES ----------------------------------- # ---------------------------------------------------- # -- Eigen add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5") if(SIMPLE_USE_DIFFCOAL) add_project_dependency(diffcoal REQUIRED PKG_CONFIG_REQUIRES) endif(SIMPLE_USE_DIFFCOAL) # -- boost set(BOOST_REQUIRED_COMPONENTS filesystem serialization system) set_boost_default_options() export_boost_default_options() add_project_dependency(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS}) # -- pinocchio add_project_dependency(pinocchio REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.9.2") # -- tracy (optional) if(SIMPLE_TRACY_ENABLE AND SIMPLE_DOWNLOAD_TRACY) # We use FetchContent_Populate because we need EXCLUDE_FROM_ALL to avoid installing Tracy with # simple. We can directly use EXCLUDE_FROM_ALL in FetchContent_Declare when CMake minimum version # will be 3.28. if(POLICY CMP0169) cmake_policy(SET CMP0169 OLD) endif() FetchContent_Declare( tracy GIT_REPOSITORY https://github.com/Simple-Robotics/tracy.git GIT_TAG patches GIT_SHALLOW TRUE GIT_PROGRESS TRUE) FetchContent_GetProperties(tracy) if(NOT tracy_POPULATED) FetchContent_Populate(tracy) set(TRACY_STATIC ON CACHE INTERNAL "") set(TRACY_ENABLE ${SIMPLE_TRACY_ENABLE} CACHE INTERNAL "") add_subdirectory(${tracy_SOURCE_DIR} ${tracy_BINARY_DIR} EXCLUDE_FROM_ALL) # Extract the target include directories, set as system get_target_property(tracy_INCLUDE_DIR TracyClient INTERFACE_INCLUDE_DIRECTORIES) set_target_properties( TracyClient PROPERTIES POSITION_INDEPENDENT_CODE True INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tracy_INCLUDE_DIR}") endif() elseif(SIMPLE_TRACY_ENABLE) # assume it is installed somewhere add_project_dependency(Tracy REQUIRED) if(NOT ${tracy_FOUND}) message( FATAL_ERROR "Simple support for tracy is enabled, but tracy was not found on your system." " Install it, or set the option SIMPLE_DOWNLOAD_TRACY to ON so we can fetch it.") else() message(STATUS "Tracy found on your system at ${Tracy_DIR}") endif() endif() # -- python if(BUILD_PYTHON_INTERFACE) message( STATUS "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." ) set(PYTHON_COMPONENTS Interpreter Development NumPy) add_project_dependency(eigenpy 2.7.10 REQUIRED) # Check wether this a PyPy Python execute_process( COMMAND ${PYTHON_EXECUTABLE} -c "import platform; print(platform.python_implementation())" OUTPUT_VARIABLE _python_implementation_value OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET) message(STATUS "Python compiler: ${_python_implementation_value}") if(_python_implementation_value MATCHES "PyPy") set(BUILD_PYTHON_INTERFACE_WITH_PYPY TRUE) endif() else(BUILD_PYTHON_INTERFACE) message( STATUS "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." ) endif(BUILD_PYTHON_INTERFACE) if(BUILD_WITH_OPENMP_SUPPORT) add_project_dependency(OpenMP REQUIRED) endif() if(BUILD_WITH_ACCELERATE_SUPPORT) if(NOT ${Eigen3_VERSION} VERSION_GREATER_EQUAL "3.4.90") message( FATAL_ERROR "Your version of Eigen is too low. Should be at least 3.4.90. Current version is ${Eigen3_VERSION}." ) endif() set(CMAKE_MODULE_PATH ${JRL_CMAKE_MODULES}/find-external/Accelerate ${CMAKE_MODULE_PATH}) # FIND_EXTERNAL "Accelerate". # # We don't export yet as there might be an issue on AMR64 platforms. find_package(Accelerate REQUIRED) message(STATUS "Build with Accelerate support framework.") add_definitions(-DPINOCCHIO_WITH_ACCELERATE_SUPPORT -DSIMPLE_WITH_ACCELERATE_SUPPORT) endif(BUILD_WITH_ACCELERATE_SUPPORT) # ---------------------------------------------------- # --- MAIN LIBRARY ----------------------------------- # ---------------------------------------------------- # Sources definition include(sources.cmake) # List headers to install list(APPEND ${PROJECT_NAME}_HEADERS ${${PROJECT_NAME}_CORE_PUBLIC_HEADERS}) if(ENABLE_TEMPLATE_INSTANTIATION) list(APPEND ${PROJECT_NAME}_CORE_SOURCES ${${PROJECT_NAME}_TEMPLATE_INSTANTIATION_SOURCES}) list(APPEND ${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${${PROJECT_NAME}_TEMPLATE_INSTANTIATION_PUBLIC_HEADERS}) endif() if(BUILD_PYTHON_INTERFACE) list(APPEND ${PROJECT_NAME}_HEADERS ${${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS}) endif() add_subdirectory(src) # ---------------------------------------------------- # --- BENCHMARK -------------------------------------- # ---------------------------------------------------- if(BUILD_BENCHMARK) add_subdirectory(benchmark) endif(BUILD_BENCHMARK) # ---------------------------------------------------- # --- BINDINGS --------------------------------------- # ---------------------------------------------------- add_subdirectory(bindings) # ---------------------------------------------------- # --- UNIT TESTS ------------------------------------- # ---------------------------------------------------- # test data config_files(tests/test_data/config.h) add_subdirectory(tests) # Export for PkgConfig pkg_config_append_libs(${PROJECT_NAME}) pkg_config_append_boost_libs(${BOOST_COMPONENTS}) ================================================ FILE: LICENSE ================================================ BSD 3-Clause License Copyright (c) 2025, INRIA Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: README.md ================================================ # The Simple Simulator: Simulation Made Simple **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. While first targetting robotics applications, **Simple** can be exploited in many other contexts: video games, system design, graphical animations, biomechanics, etc. **Simple** is developed by the [WILLOW team](https://www.di.ens.fr/willow/) at [Inria](https://www.inria.fr/en). The code associated with **Simple** has been released on May, 26th 2025 under the permissive BSD-3 license. More features and improved efficiency will come soon. And as Jean de La Fontaine wrote: **"Patience and time do more than strength or passion"**. ## The core team The following persons actively took part in the development of **Simple**: - [Justin Carpentier](https://jcarpent.github.io/) (Inria): core developer and project instigator - [Quentin Le Lidec](https://quentinll.github.io/) (Inria): core developer - [Louis Montaut](https://lmontaut.github.io/) (Inria): core developer - [Joris Vaillant](https://github.com/jorisv/) (Inria): core developer - [Yann de Mont-Marin](https://github.com/ymontmarin) (Inria): core developer - [Ajay Sathya](https://www.ajaysathya.com/) (Inria): feature contributor - [Fabian Schramm](https://github.com/fabinsch) (Inria): feature contributor External contributions are more than welcome. If you have contributed to the development of Simple, feel free to add your name. ## Associated scientific and technical publications **Simple** is built on active research around understanding and enhancing physical simulation. Interested readers can learn more about the algorithmic and computational foundations of **Simple** by reading these publications: - 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. - 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. - Sathya, A., & Carpentier, J. (2024). [Constrained Articulated Body Dynamics Algorithms](https://hal.science/hal-04443056/). IEEE Transactions on Robotics. - 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). - 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. - 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). - 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). - 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). ================================================ FILE: benchmark/CMakeLists.txt ================================================ add_project_private_dependency(benchmark REQUIRED) add_custom_target(bench) function(ADD_SIMPLE_BENCHMARK benchmark_name) add_executable(${benchmark_name} ${benchmark_name}.cpp) target_link_libraries(${benchmark_name} PRIVATE ${PROJECT_NAME}) target_link_libraries(${benchmark_name} PRIVATE benchmark::benchmark) add_dependencies(bench ${benchmark_name}) endfunction() add_simple_benchmark(mujoco-humanoid) add_simple_benchmark(affine-transform) ================================================ FILE: benchmark/affine-transform.cpp ================================================ #include #include #define N 100 #define WARMUP_TIME 1. #define MIN_TIME 2. using Vec3f = Eigen::Matrix; using Mat3f = Eigen::Matrix; using Vec4f = Eigen::Matrix; using Mat4f = Eigen::Matrix; using Mat34f = Eigen::Matrix; using Vec3d = Eigen::Matrix; using Mat3d = Eigen::Matrix; using Vec4d = Eigen::Matrix; using Mat4d = Eigen::Matrix; using Mat34d = Eigen::Matrix; using Vec3i16 = Eigen::Matrix; using Mat3i16 = Eigen::Matrix; using Vec4i16 = Eigen::Matrix; using Mat4i16 = Eigen::Matrix; using Mat34i16 = Eigen::Matrix; template struct BenchFixture : benchmark::Fixture { void SetUp(benchmark::State &) { y.reserve(N); x.reserve(N); t.reserve(N); M.reserve(N); dotprod.reserve(N); for (std::size_t i = 0; i < N; ++i) { y.push_back(ResVec::Zero()); x.push_back(InVec::Random()); t.push_back(InVec::Random()); M.push_back(Mat::Random()); dotprod.push_back(0); } } void TearDown(benchmark::State &) { } // y = sum_i M_i * x_i + t_i std::vector> y; std::vector> x; std::vector> t; std::vector> M; std::vector dotprod; }; using Bench33f = BenchFixture; BENCHMARK_DEFINE_F(Bench33f, Mat3Vec3f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].noalias() = M[i] * x[i] + t[i]; } } } BENCHMARK_REGISTER_F(Bench33f, Mat3Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using Bench33d = BenchFixture; BENCHMARK_DEFINE_F(Bench33d, Mat3Vec3d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].noalias() = M[i] * x[i] + t[i]; } } } BENCHMARK_REGISTER_F(Bench33d, Mat3Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using Bench44f = BenchFixture; BENCHMARK_DEFINE_F(Bench44f, Mat4Vec4f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].noalias() = M[i] * x[i]; } } } BENCHMARK_REGISTER_F(Bench44f, Mat4Vec4f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using Bench44d = BenchFixture; BENCHMARK_DEFINE_F(Bench44d, Mat4Vec4d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].noalias() = M[i] * x[i]; } } } BENCHMARK_REGISTER_F(Bench44d, Mat4Vec4d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using Bench34f = BenchFixture; BENCHMARK_DEFINE_F(Bench34f, Mat34Vec3f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].noalias() = M[i] * x[i]; } } } BENCHMARK_REGISTER_F(Bench34f, Mat34Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using Bench34d = BenchFixture; BENCHMARK_DEFINE_F(Bench34d, Mat34Vec3d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].noalias() = M[i] * x[i]; } } } BENCHMARK_REGISTER_F(Bench34d, Mat34Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using YannBench34f = BenchFixture; BENCHMARK_DEFINE_F(YannBench34f, Mat34Vec3f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].head<3>().noalias() = M[i] * x[i]; y[i].coeffRef(3) = 1.0f; } } } BENCHMARK_REGISTER_F(YannBench34f, Mat34Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using YannBench34d = BenchFixture; BENCHMARK_DEFINE_F(YannBench34d, Mat34Vec3d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { y[i].head<3>().noalias() = M[i] * x[i]; y[i].coeffRef(3) = 1.0f; } } } BENCHMARK_REGISTER_F(YannBench34d, Mat34Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using DotBench3f = BenchFixture; BENCHMARK_DEFINE_F(DotBench3f, Vec3f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { dotprod[i] = x[i].dot(t[i]); } } } BENCHMARK_REGISTER_F(DotBench3f, Vec3f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using DotBench3d = BenchFixture; BENCHMARK_DEFINE_F(DotBench3d, Vec3d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { dotprod[i] = x[i].dot(t[i]); } } } BENCHMARK_REGISTER_F(DotBench3d, Vec3d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using DotBench4f = BenchFixture; BENCHMARK_DEFINE_F(DotBench4f, Vec4f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { dotprod[i] = x[i].dot(t[i]); } } } BENCHMARK_REGISTER_F(DotBench4f, Vec4f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using DotBench4d = BenchFixture; BENCHMARK_DEFINE_F(DotBench4d, Vec4d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { dotprod[i] = x[i].dot(t[i]); } } } BENCHMARK_REGISTER_F(DotBench4d, Vec4d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using YannDotBench4f = BenchFixture; BENCHMARK_DEFINE_F(YannDotBench4f, Vec4f)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { dotprod[i] = (x[i].cwiseProduct(t[i])).head<3>().sum(); } } } BENCHMARK_REGISTER_F(YannDotBench4f, Vec4f)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); using YannDotBench4d = BenchFixture; BENCHMARK_DEFINE_F(YannDotBench4d, Vec4d)(benchmark::State & st) { for (auto _ : st) { for (std::size_t i = 0; i < x.size(); ++i) { dotprod[i] = (x[i].cwiseProduct(t[i])).head<3>().sum(); } } } BENCHMARK_REGISTER_F(YannDotBench4d, Vec4d)->Unit(benchmark::kMicrosecond)->MinWarmUpTime(WARMUP_TIME)->MinTime(MIN_TIME); BENCHMARK_MAIN(); ================================================ FILE: benchmark/mujoco-humanoid.cpp ================================================ // // Copyright (c) 2025 INRIA // #include "simple/core/simulator.hpp" #include "tests/test_data/config.h" #include #include #include using namespace simple; using namespace pinocchio; using ModelHandle = Simulator::ModelHandle; using DataHandle = Simulator::DataHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; using GeometryDataHandle = Simulator::GeometryDataHandle; #define ADMM ::pinocchio::ADMMContactSolverTpl #define PGS ::pinocchio::PGSContactSolverTpl const double DT = 1e-3; const std::size_t NUM_SIM_STEPS = 100; void addFloorToGeomModel(GeometryModel & geom_model) { using CollisionGeometryPtr = GeometryObject::CollisionGeometryPtr; CollisionGeometryPtr floor_collision_shape = CollisionGeometryPtr(new hpp::fcl::Halfspace(0.0, 0.0, 1.0, 0.0)); const SE3 M = SE3::Identity(); GeometryObject floor("floor", 0, 0, M, floor_collision_shape); geom_model.addGeometryObject(floor); } void addSystemCollisionPairs(const Model & model, GeometryModel & geom_model, const Eigen::VectorXd & qref) { Data data(model); GeometryData geom_data(geom_model); // TI this function to gain compilation speed on this test ::pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, qref); geom_model.removeAllCollisionPairs(); for (std::size_t i = 0; i < geom_model.geometryObjects.size(); ++i) { for (std::size_t j = i; j < geom_model.geometryObjects.size(); ++j) { if (i == j) { continue; // don't add collision pair if same object } const GeometryObject & gobj_i = geom_model.geometryObjects[i]; const GeometryObject & gobj_j = geom_model.geometryObjects[j]; if (gobj_i.name == "floor" || gobj_j.name == "floor") { // if floor, always add a collision pair geom_model.addCollisionPair(CollisionPair(i, j)); } else { if (gobj_i.parentJoint == gobj_j.parentJoint) { // don't add collision pair if same parent continue; } // run collision detection -- add collision pair if shapes are not colliding const SE3 M1 = geom_data.oMg[i]; const SE3 M2 = geom_data.oMg[j]; hpp::fcl::CollisionRequest colreq; colreq.security_margin = 1e-2; // 1cm of clearance hpp::fcl::CollisionResult colres; hpp::fcl::collide( gobj_i.geometry.get(), ::pinocchio::toFclTransform3f(M1), // gobj_j.geometry.get(), ::pinocchio::toFclTransform3f(M2), // colreq, colres); if (!colres.isCollision()) { geom_model.addCollisionPair(CollisionPair(i, j)); } } } } } struct MujocoHumanoidFixture : benchmark::Fixture { void SetUp(benchmark::State &) { ModelHandle model_handle = std::make_shared(); GeometryModelHandle geom_model_handle = std::make_shared(); Model & model = ::pinocchio::helper::get_ref(model_handle); GeometryModel & geom_model = ::pinocchio::helper::get_ref(geom_model_handle); const bool verbose = false; ::pinocchio::mjcf::buildModel(SIMPLE_TEST_DATA_DIR "/mujoco_humanoid.xml", model, verbose); ::pinocchio::mjcf::buildGeom(model, SIMPLE_TEST_DATA_DIR "/mujoco_humanoid.xml", pinocchio::COLLISION, geom_model); addFloorToGeomModel(geom_model); // initial state q0 = model.referenceConfigurations["qpos0"]; v0 = Eigen::VectorXd::Zero(model.nv); zero_torque = Eigen::VectorXd::Zero(model.nv); // add collision pairs addSystemCollisionPairs(model, geom_model, q0); sim = std::make_unique(model_handle, geom_model_handle); } void TearDown(benchmark::State &) { } std::unique_ptr sim; Eigen::VectorXd q0; Eigen::VectorXd v0; Eigen::VectorXd zero_torque; }; BENCHMARK_DEFINE_F(MujocoHumanoidFixture, admm)(benchmark::State & st) { for (auto _ : st) { Eigen::VectorXd q = q0; Eigen::VectorXd v = v0; sim->reset(); bool warm_start = st.range(0); sim->warm_start_constraints_forces = warm_start; for (size_t i = 0; i < NUM_SIM_STEPS; ++i) { sim->step(q, v, zero_torque, DT); q = sim->qnew; v = sim->vnew; } } } BENCHMARK_REGISTER_F(MujocoHumanoidFixture, admm) ->ArgName("warmstart") ->Arg(0) ->Arg(1) ->Unit(benchmark::kMillisecond) ->MinWarmUpTime(3.) ->MinTime(5.); BENCHMARK_DEFINE_F(MujocoHumanoidFixture, pgs)(benchmark::State & st) { for (auto _ : st) { Eigen::VectorXd q = q0; Eigen::VectorXd v = v0; sim->reset(); bool warm_start = st.range(0); sim->warm_start_constraints_forces = warm_start; for (size_t i = 0; i < NUM_SIM_STEPS; ++i) { sim->step(q, v, zero_torque, DT); q = sim->qnew; v = sim->vnew; } } } BENCHMARK_REGISTER_F(MujocoHumanoidFixture, pgs) ->ArgName("warmstart") ->Arg(0) ->Arg(1) ->Unit(benchmark::kMillisecond) ->MinWarmUpTime(3.) ->MinTime(5.); BENCHMARK_MAIN(); ================================================ FILE: bindings/CMakeLists.txt ================================================ # # Copyright (c) 2024 INRIA # add_subdirectory(python) if(BUILD_PYTHON_INTERFACE) set(PYWRAP ${PYWRAP} PARENT_SCOPE) endif(BUILD_PYTHON_INTERFACE) ================================================ FILE: bindings/python/CMakeLists.txt ================================================ # # Copyright (c) 2024 INRIA # include(${JRL_CMAKE_MODULES}/python-helpers.cmake) include(${JRL_CMAKE_MODULES}/stubs.cmake) # --- PYTHON TARGET --- # set(PYWRAP ${PROJECT_NAME}_pywrap) set(PYWRAP ${PYWRAP} PARENT_SCOPE) # --- COMPILE WRAPPER make_directory("${${PROJECT_NAME}_BINARY_DIR}/bindings/python/${PROJECT_NAME}") set(${PYWRAP}_SOURCES ${${PROJECT_NAME}_BINDINGS_PYTHON_SOURCES}) set(${PYWRAP}_HEADERS ${${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS}) if(BUILD_PYTHON_INTERFACE) add_custom_target(python) set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) python_build_get_target(python_build_target) add_dependencies(python ${python_build_target}) set(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.6.5") if(IS_ABSOLUTE ${PYTHON_SITELIB}) set(ABSOLUTE_PYTHON_SITELIB ${PYTHON_SITELIB}) else() set(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}) endif() set(SIMPLE_PYTHON_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME}) function(INSTALL_PYTHON_FILES) set(options) set(oneValueArgs MODULE) set(multiValueArgs FILES) cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) set(SOURCE_PATH ${PROJECT_NAME}) set(INSTALL_PATH ${SIMPLE_PYTHON_INSTALL_DIR}) if(ARGS_MODULE) set(SOURCE_PATH ${SOURCE_PATH}/${ARGS_MODULE}) set(INSTALL_PATH ${INSTALL_PATH}/${ARGS_MODULE}) endif() foreach(f ${ARGS_FILES}) python_build(${SOURCE_PATH} ${f}) install(FILES ${SOURCE_PATH}/${f} DESTINATION ${INSTALL_PATH}) endforeach() endfunction() # --- PYTHON LIB --- # set(PYTHON_LIB_NAME "${PYWRAP}") set(${PYTHON_LIB_NAME}_SOURCES ${${PYWRAP}_SOURCES}) set(${PYTHON_LIB_NAME}_HEADERS ${${PYWRAP}_HEADERS}) add_library(${PYTHON_LIB_NAME} SHARED ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS}) add_dependencies(python ${PYTHON_LIB_NAME}) set_target_properties(${PYTHON_LIB_NAME} PROPERTIES PREFIX "") # Remove lib prefix for the target # Do not report: -Wconversion as the BOOST_PYTHON_FUNCTION_OVERLOADS implicitly converts. # -Wcomment as latex equations have multi-line comments. -Wself-assign-overloaded as bp::self # operations trigger this if(NOT WIN32) target_compile_options(${PYTHON_LIB_NAME} PRIVATE -Wno-conversion -Wno-comment -Wno-self-assign-overloaded) endif(NOT WIN32) set_target_properties(${PYTHON_LIB_NAME} PROPERTIES VERSION ${PROJECT_VERSION}) add_header_group(${PYTHON_LIB_NAME}_HEADERS) add_source_group(${PYTHON_LIB_NAME}_SOURCES) modernize_target_link_libraries( ${PYTHON_LIB_NAME} SCOPE PUBLIC TARGETS eigenpy::eigenpy) target_link_libraries(${PYTHON_LIB_NAME} PUBLIC ${PROJECT_NAME}) target_compile_definitions(${PYTHON_LIB_NAME} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_HPP_FCL_PYTHON_BINDINGS) if(WIN32) target_compile_definitions(${PYTHON_LIB_NAME} PRIVATE -DNOMINMAX) target_link_libraries(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY}) endif(WIN32) target_compile_options(${PYTHON_LIB_NAME} PRIVATE -DSIMPLE_PYTHON_MODULE_NAME=${PYTHON_LIB_NAME}) set(${PYWRAP}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME}) set_target_properties( ${PYTHON_LIB_NAME} PROPERTIES PREFIX "" SUFFIX ${PYTHON_EXT_SUFFIX} OUTPUT_NAME "${PYTHON_LIB_NAME}" LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bindings/python/${PROJECT_NAME}" # On Windows, shared library are treat as binary RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bindings/python/${PROJECT_NAME}") if(UNIX AND NOT APPLE) get_relative_rpath(${${PYWRAP}_INSTALL_DIR} ${PYWRAP}_INSTALL_RPATH) set_target_properties(${PYTHON_LIB_NAME} PROPERTIES INSTALL_RPATH "${${PYWRAP}_INSTALL_RPATH}") endif() install( TARGETS ${PYTHON_LIB_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION ${SIMPLE_PYTHON_INSTALL_DIR}) install_python_files(FILES __init__.py) # --- STUBS --- # if(GENERATE_PYTHON_STUBS) python_build_get_target(python_build_target_name) load_stubgen() # Set PYWRAP and PROJECT_NAME as stubs dependencies. # # PROJECT_NAME is mandatory (even if it's a PYWRAP dependency) to find PROJECT_NAME name DLL on # windows. generate_stubs( ${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYWRAP} ${PROJECT_NAME} ${STUBGEN_DEPENDENCIES} ${python_build_target_name}) endif(GENERATE_PYTHON_STUBS) endif(BUILD_PYTHON_INTERFACE) ================================================ FILE: bindings/python/core/expose-constraints-problem.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/bindings/python/core/constraints-problem.hpp" namespace bp = boost::python; namespace simple { namespace python { void exposeConstraintsProblem() { bp::class_( "ContactMapper", "Maps a single collision pair to contact points information (contact position, contact " "normal, contact placements, constraint models/datas, friction cones, elasticities, " "penetration depths).", bp::no_init) .def_readwrite( "begin", &ConstraintsProblem::ContactMapper::begin, "The id of the first contact point for the considered collision pair.") .def_readwrite("count", &ConstraintsProblem::ContactMapper::count, "Number of contact points for the considered collision pair"); using ContactMode = ConstraintsProblem::ContactMode; bp::enum_("ContactMode") .value("BREAKING", ContactMode::BREAKING) .value("STICKING", ContactMode::STICKING) .value("SLIDING", ContactMode::SLIDING) .export_values(); // Register a handle to ConstraintsProblem using ConstraintsProblemHandle = std::shared_ptr; bp::register_ptr_to_python(); ConstraintsProblemPythonVisitor::expose(); } } // namespace python } // namespace simple ================================================ FILE: bindings/python/core/expose-contact-frame.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/contact-frame.hpp" #include "simple/bindings/python/fwd.hpp" #include namespace bp = boost::python; namespace simple { namespace python { using SE3 = ::simple::PlacementFromNormalAndPosition::SE3; using Vector3s = ::simple::PlacementFromNormalAndPosition::Vector3s; using Matrix6x3s = ::simple::PlacementFromNormalAndPosition::Matrix6x3s; SE3 placementFromNormalAndPosition(const Vector3s & normal, const Vector3s & position) { SE3 M; ::simple::PlacementFromNormalAndPosition::calc(normal, position, M); return M; } bp::tuple placementFromNormalAndPositionDerivative(const SE3 & M) { Matrix6x3s dM_dnormal; Matrix6x3s dM_dposition; ::simple::PlacementFromNormalAndPosition::calcDiff(M, dM_dnormal, dM_dposition); return bp::make_tuple(dM_dnormal, dM_dposition); } void exposeContactFrame() { bp::def( "placementFromNormalAndPosition", placementFromNormalAndPosition, bp::args("normal", "position"), "Returns a placement such that `normal` is the z-axis of the placement's rotation and " "`position` is the translation of the placement.\n" "Parameters:\n" "\tnormal: z-axis of the placement's rotation.\n" "\tposition: translation part of the placement.\n\n" "Returns: M, the placement."); bp::def( "placementFromNormalAndPositionDerivative", placementFromNormalAndPositionDerivative, bp::args("M"), "Returns the derivatives of a placement w.r.t both the normal and position than generated " "it. The normal is the z-axis of the placement's rotation and the position is the " "translation of the part of the placement." "Parameters:\n" "\tM: a placement.\n\n" "Returns: a tuple (dM_dnormal, dM_dposition), both are 6x3 matrices."); } } // namespace python } // namespace simple ================================================ FILE: bindings/python/core/expose-simulator.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/bindings/python/core/simulator.hpp" namespace simple { namespace python { void exposeSimulator() { SimulatorPythonVisitor::expose(); } } // namespace python } // namespace simple ================================================ FILE: bindings/python/module.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/bindings/python/fwd.hpp" #include "simple/config.hpp" #include "simple/fwd.hpp" #include namespace bp = boost::python; using namespace simple::python; BOOST_PYTHON_MODULE(SIMPLE_PYTHON_MODULE_NAME) { bp::docstring_options module_docstring_options(true, true, false); bp::scope().attr("__version__") = bp::str(SIMPLE_VERSION); bp::scope().attr("__raw_version__") = bp::str(SIMPLE_VERSION); eigenpy::enableEigenPy(); using Matrix6x3s = Eigen::Matrix; eigenpy::enableEigenPySpecific(); // Enable warnings bp::import("warnings"); // Dependencies bp::import("hppfcl"); bp::import("pinocchio"); exposeContactFrame(); exposeConstraintsProblem(); exposeSimulator(); } ================================================ FILE: bindings/python/simple/__init__.py ================================================ # # Copyright (c) 2024 INRIA # import pinocchio import hppfcl from .simple_pywrap import * from .simple_pywrap import __version__, __raw_version__ ================================================ FILE: include/simple/bindings/python/core/constraints-problem.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_python_core_constraints_problem_hpp__ #define __simple_python_core_constraints_problem_hpp__ #include "simple/core/constraints-problem.hpp" #include "simple/bindings/python/fwd.hpp" #include #include #define SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(type, name, desc) \ add_property( \ #name, bp::make_function(+[](Self & self) -> type { return self.name(); }), \ bp::make_function(+[](Self &, const type &) -> void { PINOCCHIO_THROW(std::invalid_argument, "Cannot manually set " #name ".") }), \ desc) namespace simple { namespace python { namespace bp = boost::python; // template // struct StdVectorOfReferenceWrappersPythonVisitor : public bp::def_visitor> // { // typedef std::vector> Self; // // public: // template // void visit(PyClass & cl) const // { // cl.def( // "__getitem__", +[](const Self & self, std::size_t i) -> const T & { return self[i].get(); }, // bp::return_value_policy()) // .def("__len__", +[](const Self & self) -> std::size_t { return self.size(); }); // } // // static void expose(const std::string & name) // { // bp::class_(name.c_str(), "Vector of reference wrappers.", bp::no_init).def(StdVectorOfReferenceWrappersPythonVisitor()); // } // }; template struct ConstraintsProblemPythonVisitor : public bp::def_visitor> { typedef typename ConstraintsProblem::Scalar Scalar; typedef ConstraintsProblem Self; using ModelHandle = typename Self::ModelHandle; using DataHandle = typename Self::DataHandle; using VectorXs = typename Self::VectorXs; using GeometryModelHandle = typename Self::GeometryModelHandle; using GeometryDataHandle = typename Self::GeometryDataHandle; using PlacementVector = typename Self::PlacementVector; using ConstraintModel = typename Self::ConstraintModel; using ConstraintData = typename Self::ConstraintData; using BilateralPointConstraintModelVector = typename Self::BilateralPointConstraintModelVector; using WeldConstraintModelVector = typename Self::WeldConstraintModelVector; template void visit(PyClass & cl) const { cl // ---------------------------------- // CONSTRUCTORS .def(bp::init( bp::args("self", "model", "data", "geom_model", "geom_data"), "Constructor")) // ---------------------------------- // ATTRIBUTES/METHODS // ---------------------------------- // -- general .def_readonly( "constraint_cholesky_decomposition", &Self::constraint_cholesky_decomposition, "Cholesky decomposition of the constraints problem. In particular, it contains the " "Cholesky decomposition of the Delassus' operator `G`.") .def_readwrite( "is_ncp", &Self::is_ncp, "Type of constraints problem. If set to true, the constraints problem is a NCP, else it is a CCP.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(int, constraints_problem_size, "Size of the full constraint problem vector.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, g, "Drift term `g` of the constraints problem.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, preconditioner, "Preconditioner of the constraints problem.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, constraints_forces, "Constraint forces of the current contact problem.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, previous_constraints_forces, "Constraint forces of the previous contact problem.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, constraints_velocities, "Constraint velocities divided by dt (v_c / dt).") .def("update", &Self::update, bp::args("self"), "Update constraints with current model, data, geom_model, geom_data.") .def("clear", &Self::clear, bp::args("self"), "Clear currrent contact quantities.") // ---------------------------------- // -- joint friction .def_readwrite("joint_friction_constraint_models", &Self::joint_friction_constraint_model, "Joint friction constraint model.") .def_readonly("joint_friction_constraint_datas", &Self::joint_friction_constraint_data, "Joint friction constraint data.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( int, joint_friction_constraint_size, "Size of the vector of dry friction force vector.") .def( "getNumberOfJointFrictionConstraints", &Self::getNumberOfJointFrictionConstraints, bp::args("self"), "Returns the number of joint friction constraints.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, joint_friction_constraint_forces, "Joint friction constraints' forces.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, previous_joint_friction_constraint_forces, "Joint friction constraints' forces at the previous time step.") // ---------------------------------- // -- bilateral constraints .def_readwrite( "bilateral_point_constraint_models", &Self::bilateral_point_constraint_models, "Vector of bilateral constraint models.") .def_readonly( "bilateral_point_constraint_datas", &Self::bilateral_point_constraint_datas, "Vector of bilateral constraint datas.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( int, bilateral_constraints_size, "Size of the vector of bilateral constraint force vector.") .def( "getNumberOfBilateralConstraints", &Self::getNumberOfBilateralConstraints, bp::args("self"), "Returns the number of bilateral constraints.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, bilateral_constraints_forces, "Bilateral constraints' forces.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, previous_bilateral_constraints_forces, "Bilateral constraints' forces at the previous time step.") // ---------------------------------- // -- weld constraints .def_readwrite("weld_constraint_models", &Self::weld_constraint_models, "Vector of weld constraint models.") .def_readonly("weld_constraint_models", &Self::weld_constraint_models, "Vector of weld constraint datas.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( int, weld_constraints_size, "Size of the vector of weld constraint force vector.") .def("getNumberOfWeldConstraints", &Self::getNumberOfWeldConstraints, bp::args("self"), "Returns the number of weld constraints.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, weld_constraints_forces, "Weld constraints' forces.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, previous_weld_constraints_forces, "Weld constraints' forces at the previous time step.") // ---------------------------------- // -- joint limits .def_readwrite("joint_limit_constraint_model", &Self::joint_limit_constraint_model, "Joint limit constraint model.") .def_readonly("joint_limit_constraint_data", &Self::joint_limit_constraint_data, "Joint limit constraint data.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(int, joint_limit_constraint_size, "Size of joint limit forces vector.") .def( "getNumberOfJointLimitConstraints", &Self::getNumberOfJointLimitConstraints, bp::args("self"), "Returns the number of joint limit constraints.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY(VectorXs, joint_limit_constraint_forces, "Joint limit constraint's forces.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, previous_joint_limit_constraint_forces, "Joint limit constraint's forces at the previous time step.") // ---------------------------------- // -- frictional point constraints .def_readwrite( "frictional_point_constraint_models", &Self::frictional_point_constraint_models, "Vector of frictional point constraint models.") .def_readonly( "frictional_point_constraint_datas", &Self::frictional_point_constraint_datas, "Vector of frictional point constraint datas.") .def( "setMaxNumberOfContactsPerCollisionPair", &Self::setMaxNumberOfContactsPerCollisionPair, bp::args("self"), "Set maximum number of contacts for each collision pair.") .def( "getMaxNumberOfContactsPerCollisionPair", &Self::getMaxNumberOfContactsPerCollisionPair, bp::args("self"), "Get the maximum number of contacts for each collision pair.") .def( "getMaxNumberOfContacts", &Self::getMaxNumberOfContacts, bp::args("self"), "Maximum number of contacts this `ConstraintsProblem` can handle.") .def("getNumberOfContacts", &Self::getNumberOfContacts, bp::args("self"), "Number of contacts.") .def("getPreviousNumberOfContacts", &Self::getPreviousNumberOfContacts, bp::args("self"), "Previous number of contacts.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( int, frictional_point_constraints_size, "Size of the vector of the frictional point constraints' forces.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, frictional_point_constraints_forces, "Frictional point constraints' forces.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, previous_frictional_point_constraints_forces, "Frictional point constraints' forces at the previous time step.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( VectorXs, frictional_point_constraints_velocities, "Frictional point constraints' velocities scaled by dt (v_c * dt).") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( PlacementVector, frictional_point_constraints_placements, "Contact placements (oMc) of the current contact problem.") .SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY( PlacementVector, previous_frictional_point_constraints_placements, "Contact placements (oMc) at the previous time step (previous contact problem).") .def_readonly("pairs_in_collision", &Self::pairs_in_collision, "Ids of collision pairs which are in collision.") .def_readonly( "contact_id_to_collision_pair", &Self::contact_id_to_collision_pair, "Vector that maps the id of the contact to the collision pair. Therefore " "`contact_id_to_collision_pair[i]` is the id of the collision pair corresponding to " "the i-th contact. Note: since collision pairs can have multiple contacts, the same id " "can be found multiple times inside `contact_id_to_collision_pair`.") .def_readonly("contact_mappers", &Self::contact_mappers, "Vector of contact mappers of the current contact problem.") .def_readonly( "contact_modes", &Self::contact_modes, "Contact modes associated to frictional point constraints (breadking, sliding or sticking).") .def( "collectActiveSet", &Self::collectActiveSet, (bp::arg("self"), bp::arg("epsilon") = 1e-6), "Collect active set of of the solution of the contact problem."); // StdVectorOfReferenceWrappersPythonVisitor::expose("StdRefVec_ConstraintModel"); // StdVectorOfReferenceWrappersPythonVisitor::expose("StdRefVec_ConstraintData"); cl.def_readonly("constraint_models", &Self::constraint_models, "Active constraint models.") .def_readonly("constraint_datas", &Self::constraint_datas, "Active constraint datas."); } static void expose() { ::pinocchio::python::StdVectorPythonVisitor::expose( "StdVec_BilateralPointConstraintModel"); ::pinocchio::python::StdVectorPythonVisitor::expose( "StdVec_WeldConstraintModel"); ::pinocchio::python::StdVectorPythonVisitor, true>::expose("StdVec_double"); ::pinocchio::python::StdVectorPythonVisitor, true>::expose( "StdVec_ContactMapper"); ::pinocchio::python::StdVectorPythonVisitor, true>::expose( "StdVec_ContactMode"); bp::class_("WrappedConstraintModel", bp::no_init) .def( "__getattr__", +[](bp::object self, std::string const & name) { using ConstraintModel = typename ConstraintsProblem::ConstraintModel; const ConstraintModel & obj = bp::extract(self()); return bp::getattr(bp::object(bp::ptr(&obj)), name.c_str()); }) .def( "__call__", +[](const typename ConstraintsProblem::WrappedConstraintModel & cmodel) -> const typename ConstraintsProblem::ConstraintModel & { return cmodel.get(); }, bp::return_internal_reference<>()); bp::class_("WrappedConstraintData", bp::no_init) .def( "__getattr__", +[](bp::object self, std::string const & name) { using ConstraintData = typename ConstraintsProblem::ConstraintData; const ConstraintData & obj = bp::extract(self()); return bp::getattr(bp::object(bp::ptr(&obj)), name.c_str()); }) .def( "__call__", +[](const typename ConstraintsProblem::WrappedConstraintData & cdata) -> const typename ConstraintsProblem::ConstraintData & { return cdata.get(); }, bp::return_internal_reference<>()); ::pinocchio::python::StdVectorPythonVisitor::expose( "StdVec_WrappedConstraintModel"); ::pinocchio::python::StdVectorPythonVisitor::expose( "StdVec_WrappedConstraintData"); bp::class_("ConstraintsProblem", "Contact problem.\n", bp::no_init) .def(ConstraintsProblemPythonVisitor()) .def(::pinocchio::python::CopyableVisitor()); } }; } // namespace python } // namespace simple #undef SIMPLE_CONSTRAINT_PROBLEM_ADD_READONLY_PROPERTY #endif // ifndef__simple_python_core_constraints_problem_hpp__ ================================================ FILE: include/simple/bindings/python/core/simulator.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_python_algorithm_simulator_hpp__ #define __simple_python_algorithm_simulator_hpp__ #include "simple/bindings/python/fwd.hpp" #include "simple/core/simulator.hpp" #include #include // clang-format off #define SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(type, name, desc) \ add_property( \ #name, \ bp::make_function( \ +[](Self & self) -> type & { return self.name(); }, bp::return_internal_reference<>()), \ bp::make_function(+[](Self &, const type &) -> void { \ PINOCCHIO_THROW(std::invalid_argument, "Cannot manually set " #name ". Use constructor instead.") \ }), \ desc) // clang-format on namespace simple { namespace python { namespace bp = boost::python; template struct SimulatorPythonVisitor : public boost::python::def_visitor> { using Self = Simulator; using Scalar = typename Self::Scalar; using ModelHandle = typename Self::ModelHandle; using Model = typename Self::Model; using DataHandle = typename Self::DataHandle; using Data = typename Self::Data; using GeometryModelHandle = typename Self::GeometryModelHandle; using GeometryModel = typename Self::GeometryModel; using GeometryDataHandle = typename Self::GeometryDataHandle; using GeometryData = typename Self::GeometryData; using BilateralPointConstraintModelVector = typename Self::BilateralPointConstraintModelVector; using WeldConstraintModelVector = typename Self::WeldConstraintModelVector; using ConstraintsProblemHandle = typename Self::ConstraintsProblemHandle; using VectorXs = typename Self::VectorXs; using MatrixXs = typename Self::MatrixXs; using SpatialForce = typename Self::SpatialForce; using SpatialForceVector = typename Self::SpatialForceVector; using ConstraintSolverType = typename Self::ConstraintSolverType; using ConstraintSolverSettings = typename Self::ConstraintSolverSettings; using ADMMConstraintSolverSettings = typename Self::ADMMConstraintSolverSettings; using PGSConstraintSolverSettings = typename Self::PGSConstraintSolverSettings; template void visit(PyClass & cl) const { cl // ----------------------------- // CONSTRUCTORS .def(bp::init( bp::args("self", "model", "data", "geom_model", "geom_data"), "Constructor")) .def(bp::init( bp::args("self", "model", "data", "geom_model", "geom_data", "bilateral_point_constraint_models"), "Constructor")) .def(bp::init( bp::args("self", "model", "data", "geom_model", "geom_data", "weld_constraint_models"), "Constructor")) .def(bp::init< ModelHandle, DataHandle, GeometryModelHandle, GeometryDataHandle, BilateralPointConstraintModelVector, WeldConstraintModelVector>( bp::args("self", "model", "data", "geom_model", "geom_data", "bilateral_point_constraint_models", "weld_constraint_models"), "Constructor")) .def(bp::init(bp::args("self", "model", "geom_model"), "Constructor")) .def(bp::init( bp::args("self", "model", "geom_model", "bilateral_point_constraint_models"), "Constructor")) .def(bp::init( bp::args("self", "model", "geom_model", "weld_constraint_models"), "Constructor")) .def(bp::init( bp::args("self", "model", "geom_model", "bilateral_point_constraint_models", "weld_constraint_models"), "Constructor")) // ----------------------------- // ATTRIBUTES .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(Model, model, "Pinocchio model.") .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(Data, data, "Pinocchio data.") .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(GeometryModel, geom_model, "Pinocchio geometry model.") .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(GeometryData, geom_data, "Pinocchio geometry data.") .SIMPLE_SIMULATOR_ADD_READONLY_HANDLE(ConstraintsProblem, constraints_problem, "Constraint problem.") // -- state .def_readonly("q", &Self::q, "Joints configuration of the system.") .def_readonly("v", &Self::v, "Joints velocity.") .def_readonly("tau", &Self::tau, "External joints torques.") .def_readonly("fext", &Self::fext, "External joints forces.") .def_readonly("dt", &Self::dt, "Time step used to compute next state.") // -- new state .def_readonly("qnew", &Self::qnew, "New joints configuration of the system.") .def_readonly("vfree", &Self::vfree, "New joints velocity as if there were no constraints.") .def_readonly("vnew", &Self::vnew, "New joints velocity.") .def_readonly("anew", &Self::anew, "New joints acceleration.") .def_readonly( "ftotal", &Self::ftotal, "Vector of total forces (external + constraint forces) applied on joints, expressed in the frame of each joint.") .def_readonly( "tau_total", &Self::tau_total, "Vector of total torques (given as input to step + constraint torques) applied on joints.") .def_readonly("tau_constraints", &Self::tau_constraints, "Vector of constraint torques.") // -- constraint solvers .def_readonly( "constraint_solver_type", &Self::constraint_solver_type, "Type of constraint solver used in the last call to `step`.") .def_readwrite("warm_start_constraints_forces", &Self::warm_start_constraints_forces, "Warm start constraint forces boolean.") .def_readwrite( "admm_constraint_solver_settings", &Self::admm_constraint_solver_settings, "Settings of the ADMM constraint solver.") .def_readwrite("admm_constraint_solver", &Self::admm_constraint_solver, "The ADMM constraints solver.") .def_readwrite("pgs_constraint_solver_settings", &Self::pgs_constraint_solver_settings, "Settings of the PGS constraint solver.") .def_readwrite("pgs_constraint_solver", &Self::pgs_constraint_solver, "The PGS constraints solver.") // -- misc .def_readwrite("measure_timings", &Self::measure_timings, "Measure timings of the `step` method.") // ----------------------------- // METHODS .def( "reset", &Self::reset, bp::args("self"), "Resets the simulator to accept a new initial state, i.e. before looping on the `step` method.") // .def( "step", +[](Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, Scalar dt, std::size_t nsteps = 1) { self.template step<::pinocchio::ADMMContactSolverTpl>(q, v, tau, dt); for (std::size_t i = 0; i < nsteps - 1; ++i) { self.template step<::pinocchio::ADMMContactSolverTpl>(self.qnew, self.vnew, tau, dt); } }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("tau"), bp::arg("dt"), bp::arg("nsteps") = 1), "Do one step of simulation with default constraint solver (ADMM).") // .def( "rollout", +[](Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, Scalar dt, std::size_t nsteps = 1) { VectorXs q_ = q; VectorXs v_ = v; VectorXs tau_ = tau; Py_BEGIN_ALLOW_THREADS; self.template step<::pinocchio::ADMMContactSolverTpl>(q_, v_, tau_, dt); for (std::size_t i = 0; i < nsteps - 1; ++i) { self.template step<::pinocchio::ADMMContactSolverTpl>(self.qnew, self.vnew, tau, dt); } Py_END_ALLOW_THREADS; }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("tau"), bp::arg("dt"), bp::arg("nsteps") = 1), " Compute a trajectory by performing a rollout of a policy. This function releases " "Python GIL so it can be parallelized (input vectors are copied for safety). TODO: in " "the future, rollout should take a control policy as input rather than a constant " "torque.") // .def( "rollout", +[](Self & self, const VectorXs & q, const VectorXs & v, const std::vector & taus, Scalar dt) { VectorXs q_ = q; VectorXs v_ = v; Py_BEGIN_ALLOW_THREADS; for (const auto & tau : taus) { self.template step<::pinocchio::ADMMContactSolverTpl>(q_, v_, tau, dt); q_ = self.qnew; v_ = self.vnew; } Py_END_ALLOW_THREADS; }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("taus"), bp::arg("dt")), "Compute a trajectory by performing a rollout of a policy with a list of taus. This function releases " "Python GIL so it can be parallelized (input vectors are copied for safety).") // .def( "rollout", +[](Self & self, const VectorXs & q, const VectorXs & v, const MatrixXs & taus, Scalar dt) { VectorXs q_ = q; VectorXs v_ = v; Py_BEGIN_ALLOW_THREADS; for (int i = 0; i < taus.rows(); ++i) { VectorXs tau = taus.row(i); self.template step<::pinocchio::ADMMContactSolverTpl>(q_, v_, tau, dt); q_ = self.qnew; v_ = self.vnew; } Py_END_ALLOW_THREADS; }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("taus"), bp::arg("dt")), "Compute a trajectory by performing a rollout of a policy with a matrix of taus. This function releases " "Python GIL so it can be parallelized (input vectors are copied for safety).") // .def( "step", +[]( Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, const SpatialForceVector & fext, Scalar dt, std::size_t nsteps = 1) { self.template step<::pinocchio::ADMMContactSolverTpl>(q, v, tau, fext, dt); for (std::size_t i = 0; i < nsteps - 1; ++i) { self.template step<::pinocchio::ADMMContactSolverTpl>(self.qnew, self.vnew, tau, fext, dt); } }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("tau"), bp::arg("fext"), bp::arg("dt"), bp::arg("nsteps") = 1), "Do one step of simulation with default constraint solver (ADMM).") // .def( "stepPGS", +[](Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, Scalar dt, std::size_t nsteps = 1) { self.template step<::pinocchio::PGSContactSolverTpl>(q, v, tau, dt); for (std::size_t i = 0; i < nsteps - 1; ++i) { self.template step<::pinocchio::PGSContactSolverTpl>(self.qnew, self.vnew, tau, dt); } }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("tau"), bp::arg("dt"), bp::arg("nsteps") = 1), "Do one step of simulation with PGS constraint solver.") // .def( "stepPGS", +[]( Self & self, const VectorXs & q, const VectorXs & v, const VectorXs & tau, const SpatialForceVector & fext, Scalar dt, std::size_t nsteps = 1) { self.template step<::pinocchio::PGSContactSolverTpl>(q, v, tau, fext, dt); for (std::size_t i = 0; i < nsteps - 1; ++i) { self.template step<::pinocchio::PGSContactSolverTpl>(self.qnew, self.vnew, tau, fext, dt); } }, (bp::arg("self"), bp::arg("q"), bp::arg("v"), bp::arg("tau"), bp::arg("fext"), bp::arg("dt"), bp::arg("nsteps") = 1), "Do one step of simulation with PGS constraint solver.") .def("getStepCPUTimes", &Self::getStepCPUTimes, bp::args("self"), "Get timings of the last call to the `step` method.") // .def( "getConstraintSolverCPUTimes", &Self::getConstraintSolverCPUTimes, bp::args("self"), "Get timings of the call to the constraint solver in the last call to the `step` method. " "These timings can be 0 if the system has no constraints.") // .def( "getCollisionDetectionCPUTimes", &Self::getCollisionDetectionCPUTimes, bp::args("self"), "Get timings of the collision detection stage."); // Register handles { // Check registration { const bp::type_info info = bp::type_id(); const bp::converter::registration * reg = bp::converter::registry::query(info); if (!reg) { bp::register_ptr_to_python(); } } { const bp::type_info info = bp::type_id(); const bp::converter::registration * reg = bp::converter::registry::query(info); if (!reg) { bp::register_ptr_to_python(); } } { const bp::type_info info = bp::type_id(); const bp::converter::registration * reg = bp::converter::registry::query(info); if (!reg) { bp::register_ptr_to_python(); } } { const bp::type_info info = bp::type_id(); const bp::converter::registration * reg = bp::converter::registry::query(info); if (!reg) { bp::register_ptr_to_python(); } } } } static void expose() { bp::class_("Simulator", "Instance of Simulator.", bp::no_init) .def(SimulatorPythonVisitor()) .def(::pinocchio::python::CopyableVisitor()); { bp::enum_("ConstraintSolverType") .value("PGS", ConstraintSolverType::PGS) .value("ADMM", ConstraintSolverType::ADMM) .value("NONE", ConstraintSolverType::NONE); } { bp::class_( "ConstraintSolverSettings", "Settings common to all constraint solvers inside `Simulator`.", bp::no_init) .def_readwrite("max_iter", &ConstraintSolverSettings::max_iter, "Max number of iteration of the constraint solver.") .def_readwrite( "absolute_precision", &ConstraintSolverSettings::absolute_precision, "Absolute convergence precision of the constraint solver.") .def_readwrite( "relative_precision", &ConstraintSolverSettings::relative_precision, "Relative convergence precision of the constraint solver.") .def_readwrite("stat_record", &ConstraintSolverSettings::stat_record, "Record metrics of the constraint solver."); } { bp::class_>( "ADMMConstraintSolverSettings", "Settings for the ADMM constraint solver inside `Simulator`.", bp::no_init) .def_readwrite("mu", &ADMMConstraintSolverSettings::mu, "Proximal parameter of ADMM.") .def_readwrite( "tau", &ADMMConstraintSolverSettings::tau, "ADMM augmented lagragian penalty is tau * rho (rho is scaled during iterations).") .def_readwrite("rho", &ADMMConstraintSolverSettings::rho, "Initial value of rho when using the linear update rule.") .def_readwrite( "rho_power", &ADMMConstraintSolverSettings::rho_power, "Initial value of rho_power when using the spectral update rule.") .def_readwrite("rho_power_factor", &ADMMConstraintSolverSettings::rho_power_factor, "Update factor on rho_power.") .def_readwrite( "linear_update_rule_factor", &ADMMConstraintSolverSettings::linear_update_rule_factor, "Update factor on rho when using linear update rule.") .def_readwrite( "ratio_primal_dual", &ADMMConstraintSolverSettings::ratio_primal_dual, "Ratio above/below which to trigger the rho update.") .def_readwrite( "lanczos_size", &ADMMConstraintSolverSettings::lanczos_size, "Size of Lanczos decomposition. Higher yields more accurate delassus eigenvalues estimates.") .def_readwrite( "admm_update_rule", &ADMMConstraintSolverSettings::admm_update_rule, "Update rule for the ADMM constraint solver (linear or spectral)"); } { bp::class_>( "PGSConstraintSolverSettings", "Settings for the PGS constraint solver inside `Simulator`.", bp::no_init) .def_readwrite("over_relax", &PGSConstraintSolverSettings::over_relax, "Optional over relaxation value, default to 1."); } } }; } // namespace python } // namespace simple #undef SIMPLE_SIMULATOR_ADD_READONLY_HANDLE #endif // ifndef __simple_python_algorithm_simulator_hpp__ ================================================ FILE: include/simple/bindings/python/fwd.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_python_fwd_hpp__ #define __simple_python_fwd_hpp__ #include namespace simple { namespace python { void exposeContactFrame(); void exposeConstraintsProblem(); void exposeSimulator(); } // namespace python } // namespace simple #endif // #ifndef __simple_python_fwd_hpp__ ================================================ FILE: include/simple/core/constraints-problem.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_constraints_problem_hpp__ #define __simple_core_constraints_problem_hpp__ #include "simple/core/fwd.hpp" #include "simple/core/contact-frame.hpp" #include #include #include #include #include #include namespace simple { template class JointCollectionTpl> struct traits> { using Scalar = _Scalar; }; template class JointCollectionTpl> struct ConstraintsProblemTpl // : ::pinocchio::NumericalBase> { // TODO: template by allocator EIGEN_MAKE_ALIGNED_OPERATOR_NEW ///////////////////////////////////////////////// /// TYPEDEFS ///////////////////////////////////////////////// using Scalar = _Scalar; enum { Options = _Options }; using GeomIndex = pinocchio::GeomIndex; using JointIndex = pinocchio::JointIndex; using VectorXs = Eigen::Matrix; using MatrixXs = Eigen::Matrix; using Vector3s = Eigen::Matrix; using Vector6s = Eigen::Matrix; using Matrix3s = Eigen::Matrix; using MapVectorXs = Eigen::Map; using MapVector3s = Eigen::Map; using MapVector6s = Eigen::Map; using ContactIndex = std::size_t; using Model = ::pinocchio::ModelTpl; using ModelHandle = std::shared_ptr; using Data = typename Model::Data; using DataHandle = std::shared_ptr; using GeometryModelHandle = std::shared_ptr<::pinocchio::GeometryModel>; using GeometryDataHandle = std::shared_ptr<::pinocchio::GeometryData>; using ConstraintModel = ::pinocchio::ConstraintModelTpl; using ConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(ConstraintModel); using WrappedConstraintModel = std::reference_wrapper; using WrappedConstraintModelVector = std::vector; using ConstraintData = ::pinocchio::ConstraintDataTpl; using ConstraintDataVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(ConstraintData); using WrappedConstraintData = std::reference_wrapper; using WrappedConstraintDataVector = std::vector; using FrictionalJointConstraintModel = ::pinocchio::FrictionalJointConstraintModelTpl; using FrictionalJointConstraintData = ::pinocchio::FrictionalJointConstraintDataTpl; using BilateralPointConstraintModel = ::pinocchio::BilateralPointConstraintModelTpl; using BilateralPointConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(BilateralPointConstraintModel); using BilateralPointConstraintData = ::pinocchio::BilateralPointConstraintDataTpl; using WeldConstraintModel = ::pinocchio::WeldConstraintModelTpl; using WeldConstraintData = ::pinocchio::WeldConstraintDataTpl; using WeldConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(WeldConstraintModel); using JointLimitConstraintModel = ::pinocchio::JointLimitConstraintModelTpl; using JointLimitConstraintData = ::pinocchio::JointLimitConstraintDataTpl; using BoxSet = ::pinocchio::BoxSetTpl; using FrictionalPointConstraintModel = ::pinocchio::FrictionalPointConstraintModelTpl; using FrictionalPointConstraintData = ::pinocchio::FrictionalPointConstraintDataTpl; using CoulombFrictionCone = ::pinocchio::CoulombFrictionConeTpl; using CoulombFrictionConeVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(CoulombFrictionCone); using ConstraintCholeskyDecomposition = ::pinocchio::ContactCholeskyDecompositionTpl; using ContactPointVector = PINOCCHIO_ALIGNED_STD_VECTOR(Vector3s); using ContactNormalVector = PINOCCHIO_ALIGNED_STD_VECTOR(Vector3s); using SE3 = ::pinocchio::SE3Tpl; using PlacementVector = PINOCCHIO_ALIGNED_STD_VECTOR(SE3); // operator to compute contact frames using PlacementFromNormalAndPosition = PlacementFromNormalAndPositionTpl; ///////////////////////////////////////////////// /// ATTRIBUTES ///////////////////////////////////////////////// /// ---------------------------------- /// General attributes public: /// \brief Cholesky decomposition of the constraints problem. ConstraintCholeskyDecomposition constraint_cholesky_decomposition; /// \brief Vector of constraint models currently active in the constraints problem. WrappedConstraintModelVector constraint_models; /// \brief Vector of constraint models' data. WrappedConstraintDataVector constraint_datas; /// \brief Wether the constraints problem should be treated as a NCP or a CCP. /// NCP = Non-linear Complementarity Problem /// CCP = Convex Complementarity Problem bool is_ncp{true}; protected: /// \brief Handle to the model of the system. ModelHandle m_model; /// \brief Handle to the model's data of the system. DataHandle m_data; /// \brief Handle to the geometry model of the system. GeometryModelHandle m_geom_model; /// \brief Handle to the geometry model's data of the system. GeometryDataHandle m_geom_data; /// \brief Drift term of the constraints problem. VectorXs m_g; /// \brief Preconditionner of the constraints problem. VectorXs m_preconditioner; /// \brief Velocity of constraints. VectorXs m_constraints_velocities; /// \brief Warm start for the constraint solver VectorXs m_constraints_velocities_warmstarts; /// \brief Holders for constraint forces (current and previous). std::array m_constraints_forces_holder; /// \brief Current constraints problem holder id. std::size_t m_current_constraints_pb_id; /// \brief Vector of time scaling factors to convert acceleration units to the units of each constraint. VectorXs m_time_scaling_acc_to_constraints; /// ---------------------------------- /// Joints dry frictions constraint public: /// \brief Frictional joint constraint model. ConstraintModel joint_friction_constraint_model; /// \brief Frictional joint constraint data. ConstraintData joint_friction_constraint_data; /// ---------------------------------- /// Bilateral constraints /// \brief Holder for bilateral point constraint models. ConstraintModelVector bilateral_point_constraint_models; /// \brief Holder for bilateral point constraint datas. ConstraintDataVector bilateral_point_constraint_datas; /// ---------------------------------- /// Weld constraints /// \brief Holder for weld point constraint models. ConstraintModelVector weld_constraint_models; /// \brief Holder for weld point constraint datas. ConstraintDataVector weld_constraint_datas; /// ---------------------------------- /// Joint limits constraint /// \brief Joint Limit constraint model. ConstraintModel joint_limit_constraint_model; /// \brief Joint Limit constraint data. ConstraintData joint_limit_constraint_data; /// ---------------------------------- /// Frictional point constraints /// \brief Holder for frictional point constraint models. ConstraintModelVector frictional_point_constraint_models; /// \brief Holder for frictional point constraint datas. ConstraintDataVector frictional_point_constraint_datas; /// \brief Vector of collision pair ids that are in collision. std::vector pairs_in_collision; /// \brief The collision pairs that were in contact at the previous time step (before update function is called). std::vector previous_colliding_collision_pairs; /// \brief Vector that maps the id of the contact to the collision pair. /// Therefore `contact_id_to_collision_pair[i]` is the id of the collision pair /// corresponding to the i-th contact. /// Note: since collision pairs can have multiple contacts, the same id can be found /// multiple times inside `contact_id_to_collision_pair`. std::vector contact_id_to_collision_pair; /// \brief Since each collision pair can have multiple contact points, this struct maps a single /// collision pair to contact points information: contact position, contact normal, contact /// placements, constraint models/datas, friction cones, elasticities and penetration depths. struct ContactMapper { ContactMapper(std::size_t begin, std::size_t count) : begin(begin) , count(count) { } /// \brief The id of the first contact point for the considered collision pair. std::size_t begin; /// \brief Number of contact points for the considered collision pair. std::size_t count; }; /// \brief Vector of contact mappers. /// The i-th element of this vector is the contact mapper for the i-th colliding collision pair. std::vector contact_mappers; /// \brief A contact can either be: /// - breaking (no contact force) /// - sticking (contact force inside the friction cone) /// - sliding (contact force saturating the friction cone) // TODO this should be moved to constraints enum struct ContactMode { BREAKING, STICKING, SLIDING, }; /// \brief vector of contact modes std::vector contact_modes; /// \brief indexes of breaking contacts std::vector breaking_contacts; /// \brief indexes of sticking contacts std::vector sticking_contacts; /// \brief indexes of sliding contacts std::vector sliding_contacts; protected: /// \brief Maximum number of contact points per collision pair. ContactIndex m_max_num_contact_per_collision_pair; /// \brief Number of contact points after `update` is called. ContactIndex m_num_contacts; /// \brief Number of contact points in previous time step. ContactIndex m_previous_num_contacts; /// \brief Holders for contact placements (current and previous). std::array m_frictional_point_constraints_placements_holder; /// \brief Size of vector of previous joint limit constraint forces. int m_previous_joint_limits_constraint_forces_size; /// \brief Active set of previous joint limit constraint model. std::vector m_previous_joint_limit_active_set; ///////////////////////////////////////////////// /// METHODS ///////////////////////////////////////////////// public: /// ---------------------------------- /// Constructors /// \brief Default constructor. ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle, const BilateralPointConstraintModelVector & bilateral_point_constraint_models, const WeldConstraintModelVector & weld_constraint_models); /// \brief Default constructor. ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle, const BilateralPointConstraintModelVector & bilateral_point_constraint_models); /// \brief Default constructor. ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle, const WeldConstraintModelVector & weld_constraint_models); /// \brief Default constructor. ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle); /// ---------------------------------- /// General methods /// \brief Returns a const reference to the model const Model & model() const { assert(this->m_model != nullptr); return pinocchio::helper::get_ref(this->m_model); } /// \brief Returns a reference to the model Model & model() { assert(this->m_model != nullptr); return pinocchio::helper::get_ref(this->m_model); } /// \brief Returns a const reference to the data const Data & data() const { assert(this->m_data != nullptr); return pinocchio::helper::get_ref(this->m_data); } /// \brief Returns a reference to the data Data & data() { assert(this->m_data != nullptr); return pinocchio::helper::get_ref(this->m_data); } /// \brief Returns a const reference to the geometry model const pinocchio::GeometryModel & geom_model() const { assert(this->m_geom_model != nullptr); return pinocchio::helper::get_ref(this->m_geom_model); } /// \brief Returns a reference to the geometry model pinocchio::GeometryModel & geom_model() { assert(this->m_geom_model != nullptr); return pinocchio::helper::get_ref(this->m_geom_model); } /// \brief Returns a const reference to the geometry data const pinocchio::GeometryData & geom_data() const { assert(this->m_geom_data != nullptr); return pinocchio::helper::get_ref(this->m_geom_data); } /// \brief Returns a reference to the geometry data pinocchio::GeometryData & geom_data() { assert(this->m_geom_data != nullptr); return pinocchio::helper::get_ref(this->m_geom_data); } /// \brief Allocates memory for the constraints problem quantities. /// Notes: /// - This method uses the the geometry model's active collision pairs to allocate memory. /// - because we always resize the constraints problem quantities, there won't be any error if /// this method is not called. /// This method is meant to optimize memory allocation for advanced users. void allocate(); /// \brief Empties constraints problem quantities. void clear(); /// \brief After `model`, `data`, `geom_model` and `geom_data` have been updated, this function /// updates `constraints`. \param compute_warm_start whether or not to compute a warm-start for /// the constraints forces, based on the previous solution of the constraints problem. void update(const bool compute_warm_start); /// \brief Build the constraints problem quantities: `G`, `g`. /// Also builds the quantities necessary to warm-start the constraint solver. /// Meant to be called after `update`. template void build( const Eigen::MatrixBase & vfree, const Eigen::MatrixBase & v, const Eigen::MatrixBase & v_warmstart, Scalar dt); /// \brief Checks consistency of the constraints problem w.r.t to its handles. bool check() const; /// \brief Size of the constraints problem. int constraints_problem_size() const { return this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size() // + this->frictional_point_constraints_size(); } /// \brief Size of the previous constraints problem. int previous_constraints_problem_size() const { return this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->previous_joint_limit_constraint_size() // + this->previous_frictional_point_constraints_size(); } /// \brief Getter for the `g` term (i.e. the free velocity of constraints + corrective terms). Eigen::VectorBlock g() { return this->m_g.head(this->constraints_problem_size()); } /// \brief const getter for the `g` term (i.e. the free velocity of constraint + corrective terms). Eigen::VectorBlock g() const { return this->m_g.head(this->constraints_problem_size()); } /// \brief Getter for the preconditioner Eigen::VectorBlock preconditioner() { return this->m_preconditioner.head(this->constraints_problem_size()); } /// \brief Const getter for the preconditioner Eigen::VectorBlock preconditioner() const { return this->m_preconditioner.head(this->constraints_problem_size()); } /// \brief Getter for the time scaling factors to convert acceleration units to the units of each constraint. Eigen::VectorBlock time_scaling_acc_to_constraints() { return this->m_time_scaling_acc_to_constraints.head(this->constraints_problem_size()); } /// \brief Const getter for the time scaling factors to convert acceleration units to the units of each constraint. Eigen::VectorBlock time_scaling_acc_to_constraints() const { return this->m_time_scaling_acc_to_constraints.head(this->constraints_problem_size()); } /// \brief Getter for constraints' forces. Eigen::VectorBlock constraints_forces() { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->constraints_problem_size()); } /// \brief Const getter for constraints' forces. Eigen::VectorBlock constraints_forces() const { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->constraints_problem_size()); } /// \brief Getter for constraints' forces at the previous time step. Eigen::VectorBlock previous_constraints_forces() { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->previous_constraints_problem_size()); } /// \brief Const getter for constraints' forces at the previous time step. Eigen::VectorBlock previous_constraints_forces() const { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->previous_constraints_problem_size()); } /// \brief Getter for constraints' velocities. /// note: this should be mutltiplied by dt to retrieve the actual constraints' velocities. Eigen::VectorBlock constraints_velocities() { return this->m_constraints_velocities.head(this->constraints_problem_size()); } /// \brief Const getter for constraints' velocities. /// note: this should be mutltiplied by dt to retrieve the actual constraints' velocities. Eigen::VectorBlock constraints_velocities() const { return this->m_constraints_velocities.head(this->constraints_problem_size()); } /// \brief Getter for the the warm-start which uses the constraints' velocities and constraint-inverse dynamics. Eigen::VectorBlock constraints_velocities_warmstarts() { return this->m_constraints_velocities_warmstarts.head(this->constraints_problem_size()); } /// \brief Const getter for the the warm-start which uses the constraints' velocities and constraint-inverse dynamics. Eigen::VectorBlock constraints_velocities_warmstarts() const { return this->m_constraints_velocities_warmstarts.head(this->constraints_problem_size()); } protected: /// \brief Updates m_current_constraints_pb_id. void updateConstraintsHolders(); /// \brief Checks consistency of the constraints' model/data vectors w.r.t geom_model and geom_data. bool checkConstraintsConsistencyWithGeometryModel() const; /// \brief Compute constraint drift g = Jc * vfree + baumgarte. /// For each constraint, the drift is expressed in the constraint formulation level /// of the constraint (position, velocity or acceleration) template void computeConstraintsDrift( const Eigen::MatrixBase & vfree, const Eigen::MatrixBase & v, const Scalar dt); public: /// ---------------------------------- /// Joints dry frictions constraints /// \brief Size of the joints dry friction force vector int joint_friction_constraint_size() const { return this->joint_friction_constraint_model.size(); } /// \brief Return the number of joint friction constraints. std::size_t getNumberOfJointFrictionConstraints() const { if (this->joint_friction_constraint_model.size() > 0) { return 1; } return 0; } /// \brief Getter for friction forces on the joints. Eigen::VectorBlock joint_friction_constraint_forces() { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size()); } /// \brief Const getter for friction forces on the joints. Eigen::VectorBlock joint_friction_constraint_forces() const { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size()); } /// \brief Getter for previous friction forces on the joints. Eigen::VectorBlock previous_joint_friction_constraint_forces() { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size()); } /// \brief Const getter for previous friction forces on the joints. Eigen::VectorBlock previous_joint_friction_constraint_forces() const { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].head(this->joint_friction_constraint_size()); } /// ---------------------------------- /// Bilateral constraints /// \brief Size of the bilateral constraints. int bilateral_constraints_size() const { return (int)(3 * this->bilateral_point_constraint_models.size()); } /// \brief Return the number of bilateral constraints. std::size_t getNumberOfBilateralConstraints() const { return this->bilateral_point_constraint_models.size(); } /// \brief Getter for bilateral constraints' forces. Eigen::VectorBlock bilateral_constraints_forces() { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size(), this->bilateral_constraints_size()); } /// \brief Const getter for bilateral constraints' forces. Eigen::VectorBlock bilateral_constraints_forces() const { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size(), this->bilateral_constraints_size()); } /// \brief Getter for bilateral constraints' forces. Eigen::VectorBlock previous_bilateral_constraints_forces() { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size(), this->bilateral_constraints_size()); } /// \brief Const getter for bilateral constraints' forces. Eigen::VectorBlock previous_bilateral_constraints_forces() const { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size(), this->bilateral_constraints_size()); } /// ---------------------------------- /// Weld constraints /// \brief Size of the weld constraints. int weld_constraints_size() const { return (int)(6 * this->weld_constraint_models.size()); } /// \brief Return the number of weld constraints. std::size_t getNumberOfWeldConstraints() const { return this->weld_constraint_models.size(); } /// \brief Getter for weld constraints' forces. Eigen::VectorBlock weld_constraints_forces() { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size()); } /// \brief Const getter for weld constraints' forces. Eigen::VectorBlock weld_constraints_forces() const { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size()); } /// \brief Getter for previous weld constraints' forces. Eigen::VectorBlock previous_weld_constraints_forces() { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size()); } /// \brief Const getter for previous weld constraints' forces. Eigen::VectorBlock previous_weld_constraints_forces() const { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size(), this->weld_constraints_size()); } /// ---------------------------------- /// Joint limits constraint /// \brief Maximum size of the joints limit force vector. int joint_limit_constraint_max_size() const { return this->joint_limit_constraint_model.size(); } /// \brief Size of the joints limit force vector. /// This corresponds to the size of the currently active joint limit constraint (i.e. the distance between the /// joint's position and the joint's limits is below a threshold). /// This requires the `update` function to be called before (as it itself calls `calc` on the joint limit constraint models/datas). int joint_limit_constraint_size() const { return this->joint_limit_constraint_model.activeSize(); } /// \brief Previous size of the joints limit force vector. int previous_joint_limit_constraint_size() const { return int(this->m_previous_joint_limits_constraint_forces_size); } /// \brief Return the number of joint limit constraint. std::size_t getNumberOfJointLimitConstraints() const { if (this->joint_limit_constraint_model.activeSize() > 0) { return 1; } return 0; } /// \brief Getter for forces from the limits on the joints. Eigen::VectorBlock joint_limit_constraint_forces() { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(), this->joint_limit_constraint_size()); } /// \brief Const getter for forces from the limits on the joints. Eigen::VectorBlock joint_limit_constraint_forces() const { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(), this->joint_limit_constraint_size()); } /// \brief Getter for forces from the previous limits on the joints. Eigen::VectorBlock previous_joint_limit_constraint_forces() { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(), this->previous_joint_limit_constraint_size()); } /// \brief Getter for forces from the previous limits on the joints. Eigen::VectorBlock previous_joint_limit_constraint_forces() const { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() + this->bilateral_constraints_size() + this->weld_constraints_size(), this->previous_joint_limit_constraint_size()); } /// ---------------------------------- /// Frictional point constraints /// \brief Size of vector of frictional point constraints' forces. /// For example, if only 3D contacts are considered, this should be 3 * nc, where nc is the /// number of contacts obtained after calling the `update` method. int frictional_point_constraints_size() const { return (int)(3 * this->m_num_contacts); } /// \brief Size of the previous contact problem. int previous_frictional_point_constraints_size() const { return (int)(3 * this->m_previous_num_contacts); } /// \brief Sets the maximum number of contact points per collision pair. void setMaxNumberOfContactsPerCollisionPair(ContactIndex num) { if (num <= 0) { PINOCCHIO_THROW_PRETTY(std::logic_error, "Cannot set max number of contacts per collision pair to a value <= 0."); } this->m_max_num_contact_per_collision_pair = num; this->allocate(); } /// \brief Sets the maximum number of contact points per collision pair. ContactIndex getMaxNumberOfContactsPerCollisionPair() const { return this->m_max_num_contact_per_collision_pair; } /// \brief Returns the maximum number of contact points this `ConstraintsProblem` can handle. /// This number determines the allocated memory of the `ConstraintsProblem`. /// To update the maximum number of contact points, please update the geom_model and call the /// `allocate` method to compute the updated maximum number of contact points. ContactIndex getMaxNumberOfContacts() const { return this->frictional_point_constraint_models.size(); } /// \brief Returns the number of contact points after `update` is called. ContactIndex getNumberOfContacts() const { return this->m_num_contacts; } /// \brief Returns the previous number of contact points after `update` is called. ContactIndex getPreviousNumberOfContacts() const { return this->m_previous_num_contacts; } /// \brief Getter for frictional point constraints' forces. Eigen::VectorBlock frictional_point_constraints_forces() { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Const getter for frictional point constraints' forces. Eigen::VectorBlock frictional_point_constraints_forces() const { return this->m_constraints_forces_holder[this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Getter for frictional point constraints' forces at the previous time step. Eigen::VectorBlock previous_frictional_point_constraints_forces() { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->previous_joint_limit_constraint_size(), this->previous_frictional_point_constraints_size()); } /// \brief Const getter for frictional point constraints' forces at the previous time step. Eigen::VectorBlock previous_frictional_point_constraints_forces() const { return this->m_constraints_forces_holder[1 - this->m_current_constraints_pb_id].segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->previous_joint_limit_constraint_size(), this->previous_frictional_point_constraints_size()); } /// \brief Getter for frictional point constraints' velocities. /// note: this should be mutltiplied by dt to retrieve the actual velocities. Eigen::VectorBlock frictional_point_constraints_velocities() { return this->m_constraints_velocities.segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Const getter for frictional point constraints' velocities. /// note: this should be mutltiplied by dt to retrieve the actual velocities. Eigen::VectorBlock frictional_point_constraints_velocities() const { return this->m_constraints_velocities.segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Getter for the the warm-start which uses the frictional point constraints' velocities and contact-inverse dynamics. Eigen::VectorBlock frictional_point_constraints_warmstart() { return this->m_constraints_velocities_warmstarts.segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Const getter for the the warm-start which uses the frictional point constraints' velocities and contact-inverse dynamics. Eigen::VectorBlock frictional_point_constraints_warmstart() const { return this->m_constraints_velocities_warmstarts.segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Getter for the time scaling factors to convert acceleration units to the units of each constraint. Eigen::VectorBlock contact_time_scaling_acc_to_constraints() { return this->m_time_scaling_acc_to_constraints.segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Const getter for the time scaling factors to convert acceleration units to the units of each constraint. Eigen::VectorBlock contact_time_scaling_acc_to_constraints() const { return this->m_time_scaling_acc_to_constraints.segment( this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_size(), this->frictional_point_constraints_size()); } /// \brief Getter for contact placements. PlacementVector & frictional_point_constraints_placements() { return this->m_frictional_point_constraints_placements_holder[this->m_current_constraints_pb_id]; } /// \brief Const getter for contact placements. const PlacementVector & frictional_point_constraints_placements() const { return this->m_frictional_point_constraints_placements_holder[this->m_current_constraints_pb_id]; } /// \brief Getter for contact placements at the previous time step. PlacementVector & previous_frictional_point_constraints_placements() { return this->m_frictional_point_constraints_placements_holder[1 - this->m_current_constraints_pb_id]; } /// \brief Const getter for contact placements at the previous time step. const PlacementVector & previous_frictional_point_constraints_placements() const { return this->m_frictional_point_constraints_placements_holder[1 - this->m_current_constraints_pb_id]; } /// \brief Collecting active set from the solution of the contact problem. /// the contact problem should be solved before calling this method. void collectActiveSet(Scalar epsilon = 1e-6); }; } // namespace simple /* --- Details -------------------------------------------------------------- */ #include "simple/core/constraints-problem.hxx" #if SIMPLE_ENABLE_TEMPLATE_INSTANTIATION #include "simple/core/constraints-problem.txx" #endif #endif // ifndef __simple_core_constraints_problem_hpp__ ================================================ FILE: include/simple/core/constraints-problem.hxx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_constraints_problem_hxx__ #define __simple_core_constraints_problem_hxx__ #include "simple/core/constraints-problem.hpp" #include "simple/tracy.hpp" #include "simple/utils/visitors.hpp" #include #include #include #include #include #include #include namespace simple { // -------------------------------------------------------------------------- template class JointCollectionTpl> ConstraintsProblemTpl::ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle, const BilateralPointConstraintModelVector & bilateral_point_constraint_models, const WeldConstraintModelVector & weld_constraint_models) : m_model(model_handle) , m_data(data_handle) , m_geom_model(geom_model_handle) , m_geom_data(geom_data_handle) , m_current_constraints_pb_id(0) , m_max_num_contact_per_collision_pair(4) , m_num_contacts(0) , m_previous_num_contacts(0) , m_previous_joint_limits_constraint_forces_size(0) { for (std::size_t i = 0; i < bilateral_point_constraint_models.size(); ++i) { this->bilateral_point_constraint_models.emplace_back(bilateral_point_constraint_models[i]); this->bilateral_point_constraint_datas.emplace_back(bilateral_point_constraint_models[i].createData()); } for (std::size_t i = 0; i < weld_constraint_models.size(); ++i) { this->weld_constraint_models.emplace_back(weld_constraint_models[i]); this->weld_constraint_datas.emplace_back(weld_constraint_models[i].createData()); } this->allocate(); } // -------------------------------------------------------------------------- template class JointCollectionTpl> ConstraintsProblemTpl::ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle) : ConstraintsProblemTpl( model_handle, // data_handle, // geom_model_handle, // geom_data_handle, // BilateralPointConstraintModelVector(), // WeldConstraintModelVector()) { } template class JointCollectionTpl> ConstraintsProblemTpl::ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle, const BilateralPointConstraintModelVector & bilateral_point_constraint_models) : ConstraintsProblemTpl( model_handle, // data_handle, // geom_model_handle, // geom_data_handle, // bilateral_point_constraint_models, // WeldConstraintModelVector()) { } template class JointCollectionTpl> ConstraintsProblemTpl::ConstraintsProblemTpl( const ModelHandle & model_handle, const DataHandle & data_handle, const GeometryModelHandle & geom_model_handle, const GeometryDataHandle & geom_data_handle, const WeldConstraintModelVector & weld_constraint_models) : ConstraintsProblemTpl( model_handle, // data_handle, // geom_model_handle, // geom_data_handle, // BilateralPointConstraintModelVector(), // weld_constraint_models) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> void ConstraintsProblemTpl::clear() { // Clearing joint limits this->m_previous_joint_limits_constraint_forces_size = this->joint_limit_constraint_size(); JointLimitConstraintModel * jlcm = boost::get(&this->joint_limit_constraint_model); assert(jlcm != nullptr); if (jlcm != nullptr) { this->m_previous_joint_limit_active_set = jlcm->getActiveSetIndexes(); } // Clearing contacts this->pairs_in_collision.clear(); this->contact_id_to_collision_pair.clear(); this->frictional_point_constraints_placements().clear(); this->m_previous_num_contacts = this->m_num_contacts; this->m_num_contacts = 0; // Clearing constraint models corresponding to joint limits and contacts std::size_t begin_idx_to_erase = this->getNumberOfJointFrictionConstraints(); begin_idx_to_erase += this->getNumberOfBilateralConstraints(); begin_idx_to_erase += this->getNumberOfWeldConstraints(); this->constraint_models.erase(this->constraint_models.begin() + (int)begin_idx_to_erase, this->constraint_models.end()); this->constraint_datas.erase(this->constraint_datas.begin() + (int)begin_idx_to_erase, this->constraint_datas.end()); } // -------------------------------------------------------------------------- template class JointCollectionTpl> void ConstraintsProblemTpl::allocate() { SIMPLE_TRACY_ZONE_SCOPED_N("ConstraintsProblem::allocate"); // // We first allocate memory for the constraints by setting their holders. // // -- Joint friction // first we check which joint actually has friction: int size_dry_friction = 0; std::vector active_friction_joints_ids; std::vector dry_friction_joint_idx_v; for (std::size_t i = 1; i < (std::size_t)this->model().njoints; ++i) { const pinocchio::JointModel & joint = this->model().joints[i]; if ( !(this->model().upperDryFrictionLimit.segment(joint.idx_v(), joint.nv()).isZero()) || !(this->model().lowerDryFrictionLimit.segment(joint.idx_v(), joint.nv()).isZero())) { dry_friction_joint_idx_v.push_back(size_dry_friction); active_friction_joints_ids.push_back(joint.id()); size_dry_friction += joint.nv(); } } // now we can build the holder for the frictional joint constraint VectorXs lowerDryFrictionLimit = VectorXs::Zero(size_dry_friction); VectorXs upperDryFrictionLimit = VectorXs::Zero(size_dry_friction); for (std::size_t i = 0; i < active_friction_joints_ids.size(); ++i) { const JointIndex joint_id = active_friction_joints_ids[i]; const pinocchio::JointModel & joint = this->model().joints[joint_id]; lowerDryFrictionLimit.segment(dry_friction_joint_idx_v[i], joint.nv()) = this->model().lowerDryFrictionLimit.segment(joint.idx_v(), joint.nv()); upperDryFrictionLimit.segment(dry_friction_joint_idx_v[i], joint.nv()) = this->model().upperDryFrictionLimit.segment(joint.idx_v(), joint.nv()); } FrictionalJointConstraintModel fjcm(this->model(), active_friction_joints_ids); fjcm.set() = BoxSet(lowerDryFrictionLimit, upperDryFrictionLimit); this->joint_friction_constraint_model = fjcm; this->joint_friction_constraint_data = fjcm.createData(); // -- Bilateral // they have been filled in the constructor // -- Weld // they have been filled in the constructor // -- Joint limits // first we check which joint actually has limits: std::vector active_limit_joints_ids; for (std::size_t i = 1; i < (std::size_t)this->model().njoints; ++i) { const pinocchio::JointModel & joint = this->model().joints[i]; if (joint.nq() == 1 && joint.hasConfigurationLimit()[0]) { active_limit_joints_ids.push_back(joint.id()); } // TODO: understand why we need to force nq==1 // for (std::size_t j = 0; j < (std::size_t)joint.nq(); ++j) // { // bool has_limit = joint.hasConfigurationLimit()[j]; // if (has_limit) // { // active_limit_joints_ids.push_back(joint.id()); // break; // } // } } // now we can build the holder for the joint limits constraints // TODO check that we use the margins contained in the model JointLimitConstraintModel jlcm(this->model(), active_limit_joints_ids); this->joint_limit_constraint_model = jlcm; this->joint_limit_constraint_data = jlcm.createData(); this->m_previous_joint_limit_active_set.reserve(std::size_t(this->joint_limit_constraint_max_size())); // -- Frictional contacts // For now, it's assumed that there is only one `hpp::fcl::Contact` per collision pair, meaning // that although there can be multiple contact points between two shapes, there can only be one // normal (and thus one contact patch for each collision pair). Note: each `Contact` has its own // `ContactPatch`. In the future, if we support multiple contact normals for a single collision // pair (i.e. for two non-convex objects), we will have to recompute `m_max_num_contacts`, by // factoring the number of `Contact` per collision pair. assert(this->geom_data().collisionRequests.size() == this->geom_model().collisionPairs.size()); assert(this->geom_data().collisionRequests.size() == this->geom_data().collisionResults.size()); std::size_t max_num_contacts = this->geom_model().collisionPairs.size() * this->m_max_num_contact_per_collision_pair; const int frictional_contact_max_size = (int)(3 * max_num_contacts); this->pairs_in_collision.reserve(this->geom_model().collisionPairs.size()); this->contact_id_to_collision_pair.reserve(max_num_contacts); this->frictional_point_constraints_placements().reserve(max_num_contacts); this->previous_frictional_point_constraints_placements().reserve(max_num_contacts); this->previous_colliding_collision_pairs.assign(this->geom_model().collisionPairs.size(), false); this->contact_mappers.assign(this->geom_model().collisionPairs.size(), ContactMapper(0, 0)); this->breaking_contacts.reserve(max_num_contacts); this->sticking_contacts.reserve(max_num_contacts); this->sliding_contacts.reserve(max_num_contacts); this->contact_modes.reserve(max_num_contacts); this->frictional_point_constraint_models.reserve(max_num_contacts); this->frictional_point_constraint_datas.reserve(max_num_contacts); this->frictional_point_constraint_models.clear(); this->frictional_point_constraint_datas.clear(); const ::pinocchio::FrictionCoefficientMatrix & friction_coefficients = ::pinocchio::getFrictionCoefficientMatrix(); for (::pinocchio::PairIndex col_pair_id = 0; col_pair_id < this->geom_model().collisionPairs.size(); ++col_pair_id) { // add contact models/datas const GeomIndex geom_id1 = this->geom_model().collisionPairs[col_pair_id].first; const GeomIndex geom_id2 = this->geom_model().collisionPairs[col_pair_id].second; const ::pinocchio::GeometryObject & geom1 = this->geom_model().geometryObjects[geom_id1]; const ::pinocchio::GeometryObject & geom2 = this->geom_model().geometryObjects[geom_id2]; const ::pinocchio::JointIndex joint_id1 = geom1.parentJoint; const ::pinocchio::JointIndex joint_id2 = geom2.parentJoint; // fill collision pairs frictions and elasticities const ::pinocchio::PhysicsMaterial & material1 = geom1.physicsMaterial; const ::pinocchio::PhysicsMaterial & material2 = geom2.physicsMaterial; const Scalar friction = static_cast(friction_coefficients.getFrictionFromMaterialPair(material1.materialType, material2.materialType)); const Scalar compliance = static_cast(Scalar(0.5) * (material1.compliance + material2.compliance)); // const Scalar elasticity = static_cast(0.5 * (material1.elasticity + material2.elasticity)); for (ContactIndex contact_id = 0; contact_id < this->m_max_num_contact_per_collision_pair; ++contact_id) { FrictionalPointConstraintModel fpcm(this->model(), joint_id1, SE3::Identity(), joint_id2, SE3::Identity()); const std::string name = "FrictionalPointConstraintModel_" + std::to_string(geom_id1) + "_" + std::to_string(geom_id2); fpcm.name = name; fpcm.compliance().setConstant(compliance); fpcm.desired_constraint_offset = Vector3s(0, 0, this->geom_data().collisionRequests[col_pair_id].security_margin); fpcm.set() = CoulombFrictionCone(friction); this->frictional_point_constraint_models.emplace_back(fpcm); this->frictional_point_constraint_models.back().name = name; this->frictional_point_constraint_datas.emplace_back(fpcm.createData()); } } assert(this->frictional_point_constraint_models.size() == max_num_contacts); assert(this->frictional_point_constraint_datas.size() == max_num_contacts); // // Now that all constraints have been listed, we can allocate the maximum memory for the constraint models and datas references. // These references will point to the data stored in the constraints models/data that are stored in the holders. // Certain constraints like frictional point constraints are dynamic, meaning that at a certain // time step they can be active or inactive. // Hence, after these constraints have been added to constraint_models/datas and that memory // has been allocated, they are removed from constraint_models/data. // It's then the job of the `update` method to deal with dynamic constraints. // std::size_t max_num_constraints = max_num_contacts + this->bilateral_point_constraint_models.size(); max_num_constraints += this->weld_constraint_models.size(); // TODO this is wrong, we can have more than one joint friction constraint or one joint limit constraint max_num_constraints += this->joint_friction_constraint_size() > 0 ? 1 : 0; max_num_constraints += this->joint_limit_constraint_size() > 0 ? 1 : 0; this->constraint_models.reserve(max_num_constraints); this->constraint_datas.reserve(max_num_constraints); this->constraint_models.clear(); this->constraint_datas.clear(); // variable used to make sure we allocate enough data to hold any constraint jacobian int max_constraint_size = this->geom_model().collisionPairs.size() > 0 ? 3 : 0; if (this->joint_friction_constraint_model.size() > 0) { this->constraint_models.emplace_back(this->joint_friction_constraint_model); this->constraint_datas.emplace_back(this->joint_friction_constraint_data); max_constraint_size = std::max(this->joint_friction_constraint_size(), max_constraint_size); } if (this->bilateral_point_constraint_models.size() > 0) { for (std::size_t i = 0; i < this->bilateral_point_constraint_models.size(); ++i) { this->constraint_models.emplace_back(this->bilateral_point_constraint_models[i]); this->constraint_datas.emplace_back(this->bilateral_point_constraint_datas[i]); } max_constraint_size = std::max(3, max_constraint_size); } if (this->weld_constraint_models.size() > 0) { for (std::size_t i = 0; i < this->weld_constraint_models.size(); ++i) { this->constraint_models.emplace_back(this->weld_constraint_models[i]); this->constraint_datas.emplace_back(this->weld_constraint_datas[i]); } max_constraint_size = std::max(6, max_constraint_size); } if (this->joint_limit_constraint_model.size() > 0) { this->constraint_models.emplace_back(this->joint_limit_constraint_model); this->constraint_datas.emplace_back(this->joint_limit_constraint_data); max_constraint_size = std::max(this->joint_limit_constraint_max_size(), max_constraint_size); } if (this->frictional_point_constraint_models.size() > 0) { for (std::size_t i = 0; i < this->frictional_point_constraint_models.size(); ++i) { this->constraint_models.emplace_back(this->frictional_point_constraint_models[i]); this->constraint_datas.emplace_back(this->frictional_point_constraint_datas[i]); } max_constraint_size = std::max(3, max_constraint_size); } const int constraints_problem_max_size = this->joint_friction_constraint_size() // + this->bilateral_constraints_size() // + this->weld_constraints_size() // + this->joint_limit_constraint_max_size() // + frictional_contact_max_size; // // allocate the rest of the ConstraintsProblem this->constraint_cholesky_decomposition.resize(this->model(), this->constraint_models); this->m_g.resize(constraints_problem_max_size); this->m_preconditioner.resize(constraints_problem_max_size); this->m_time_scaling_acc_to_constraints.resize(constraints_problem_max_size); this->m_constraints_forces_holder[0].resize(constraints_problem_max_size); this->m_constraints_forces_holder[1].resize(constraints_problem_max_size); this->m_constraints_velocities_warmstarts.resize(constraints_problem_max_size); this->m_constraints_velocities.resize(constraints_problem_max_size); // initialize constraints forces this->constraints_forces().setZero(); this->previous_constraints_forces().setZero(); this->clear(); // remove frictional contact models from constraints vector } // -------------------------------------------------------------------------- template class JointCollectionTpl> void ConstraintsProblemTpl::update(const bool compute_warm_start) { SIMPLE_TRACY_ZONE_SCOPED_N("ConstraintsProblem::update"); this->updateConstraintsHolders(); this->clear(); assert(this->m_num_contacts == 0 && "The number of contacts should be 0."); assert(this->geom_data().collisionResults.size() == geom_model().collisionPairs.size()); assert(this->checkConstraintsConsistencyWithGeometryModel()); // Process joint limit constraints to check which ones are active ConstraintModel & constraint_model = this->joint_limit_constraint_model; ConstraintData & constraint_data = this->joint_limit_constraint_data; JointLimitConstraintModel * jlcm = boost::get(&constraint_model); assert(jlcm != nullptr); if (jlcm != nullptr) { JointLimitConstraintData * jlcd = boost::get(&constraint_data); assert(jlcd != nullptr); jlcm->resize(this->model(), this->data(), *jlcd); } constraint_model.calc(this->model(), this->data(), constraint_data); int active_size = constraint_model.activeSize(); if (active_size > 0) { this->joint_limit_constraint_forces().setZero(); if (compute_warm_start && this->previous_joint_limit_constraint_size() > 0) { // Warm-start the joint limits constraint forces. // We assume that the active set indexes are sorted. // We search in the current active set which index belongs to the previous active set. const std::vector & prev_active_set = this->m_previous_joint_limit_active_set; assert(prev_active_set.empty() == false); const std::vector & active_set = jlcm->getActiveSetIndexes(); std::size_t j = 0; for (std::size_t i = 0; i < active_set.size(); ++i) { const std::size_t idx_active = active_set[i]; for (; j < prev_active_set.size(); ++j) { const std::size_t idx_prev_active = prev_active_set[j]; if (idx_prev_active > idx_active) { break; } if (idx_prev_active == idx_active) { this->joint_limit_constraint_forces()[int(i)] = this->previous_joint_limit_constraint_forces()[int(j)]; break; } } } } this->constraint_models.emplace_back(constraint_model); this->constraint_datas.emplace_back(constraint_data); } ContactIndex col_pair_contact_model_begin_id = 0; // Tracks the id of the first constraint model of the current collision pair for (GeomIndex col_pair_id = 0; col_pair_id < this->geom_model().collisionPairs.size(); col_pair_id++) { // TODO(louis): check if pair is active const hpp::fcl::CollisionResult & col_res = this->geom_data().collisionResults[col_pair_id]; const hpp::fcl::ContactPatchResult & patch_res = this->geom_data().contactPatchResults[col_pair_id]; // TODO(louis): extend to deal with multiple contact patches per collision pair. if (col_res.isCollision() && patch_res.numContactPatches() > 0) { this->pairs_in_collision.emplace_back(col_pair_id); const GeomIndex geom_id1 = this->geom_model().collisionPairs[col_pair_id].first; const GeomIndex geom_id2 = this->geom_model().collisionPairs[col_pair_id].second; const ::pinocchio::GeometryObject & geom1 = this->geom_model().geometryObjects[geom_id1]; const ::pinocchio::GeometryObject & geom2 = this->geom_model().geometryObjects[geom_id2]; const JointIndex joint_id1 = geom1.parentJoint; const JointIndex joint_id2 = geom2.parentJoint; const SE3 & oMi1 = this->data().oMi[joint_id1]; const SE3 & oMi2 = this->data().oMi[joint_id2]; // For the moment, we consider only one contact patch per collision pair. // All the contact points of the patch share the same normal. const hpp::fcl::ContactPatch & patch = patch_res.getContactPatch(0); const Vector3s & contact_normal = patch.getNormal(); const std::size_t num_contacts_colpair = std::min(patch.size(), this->m_max_num_contact_per_collision_pair); // All the points of the patch share the same normal; the rotation part of the contact frame // does not need to be recomputed for every point of the patch. SE3 oMc; PlacementFromNormalAndPosition::calc(contact_normal, Vector3s::Zero(), oMc); for (ContactIndex contact_id = 0; contact_id < num_contacts_colpair; ++contact_id) { this->contact_id_to_collision_pair.push_back(col_pair_id); // oMc.translation() = patch.getPoint(contact_id); this->frictional_point_constraints_placements().emplace_back(oMc); const SE3 i1Mc = oMi1.actInv(oMc); // SE3 & oMc1 = oMc; oMc1.translation() = patch.getPointShape1(contact_id); const SE3 i1Mc1 = oMi1.actInv(oMc1); // SE3 & oMc2 = oMc; oMc2.translation() = patch.getPointShape2(contact_id); const SE3 i2Mc2 = oMi2.actInv(oMc2); // Update constraint models and datas const std::size_t constraint_model_id = col_pair_contact_model_begin_id + contact_id; FrictionalPointConstraintModel * fpcmodel = boost::get(&(this->frictional_point_constraint_models[constraint_model_id])); if (fpcmodel == nullptr) { PINOCCHIO_THROW_PRETTY(std::runtime_error, "Invalid constraint model type."); } else { fpcmodel->joint1_placement = i1Mc1; fpcmodel->joint2_placement = i2Mc2; } // Note: friction is set in `allocate` when constructing the frictional point constraints. // The friction is retrieved from the materials of the geometries. // The same can be done for elasticity. const ConstraintModel & constraint_model = this->frictional_point_constraint_models[constraint_model_id]; ConstraintData & constraint_data = this->frictional_point_constraint_datas[constraint_model_id]; this->constraint_models.emplace_back(constraint_model); this->constraint_datas.emplace_back(constraint_data); constraint_model.calc(this->model(), this->data(), constraint_data); #ifndef NDEBUG // sanity check const ::coal::CollisionRequest & col_req = this->geom_data().collisionRequests[col_pair_id]; const FrictionalPointConstraintData * fpcdata = boost::get(&constraint_data); assert(fpcdata != nullptr); assert( std::abs((fpcdata->constraint_position_error - Vector3s(0, 0, patch.penetration_depth - col_req.security_margin)).norm()) < std::sqrt(Eigen::NumTraits::dummy_precision())); #endif // NDEBUG // Warm-start the contact forces const auto cindex = static_cast(3 * this->m_num_contacts); Vector3s warm_start_force = Vector3s::Zero(); if (compute_warm_start && this->previous_colliding_collision_pairs[col_pair_id]) { Scalar closest_previous_contact_distance = std::numeric_limits::infinity(); ContactIndex closest_previous_contact_id = this->contact_mappers[col_pair_id].begin; SE3 cprevMc; for (ContactIndex previous_contact_id = this->contact_mappers[col_pair_id].begin; previous_contact_id < this->contact_mappers[col_pair_id].begin + this->contact_mappers[col_pair_id].count; ++previous_contact_id) { // TODO(quentin) search which previous contact corresponds to the current one in a more clever way const SE3 & i1Mc_previous = this->previous_frictional_point_constraints_placements()[previous_contact_id]; const SE3 cprevMc_tmp = i1Mc_previous.actInv(i1Mc); Scalar previous_contact_distance = (i1Mc.translation() - i1Mc_previous.translation()).norm(); if (previous_contact_distance < closest_previous_contact_distance) { closest_previous_contact_distance = previous_contact_distance; closest_previous_contact_id = previous_contact_id; cprevMc = cprevMc_tmp; } } const auto cprev_index = static_cast(3 * closest_previous_contact_id); warm_start_force.noalias() = cprevMc.rotation() * this->previous_frictional_point_constraints_forces().template segment<3>( cprev_index); // TODO(quentin) check this is properly done } // Important: first update the number of contact, then set the eigen vectors like // frictional_point_constraints_forces, in this order. ++(this->m_num_contacts); this->frictional_point_constraints_forces().template segment<3>(cindex) = warm_start_force; } // update previous_colliding_collision_pairs and m_contact_mappers with newly created contacts this->previous_colliding_collision_pairs[col_pair_id] = true; // The role of the contact mapper is to map a collision pair to its number of contact points. this->contact_mappers[col_pair_id].begin = this->m_num_contacts - num_contacts_colpair; this->contact_mappers[col_pair_id].count = num_contacts_colpair; } else { this->contact_mappers[col_pair_id].begin = 0; this->contact_mappers[col_pair_id].count = 0; // nothing to map. this->previous_colliding_collision_pairs[col_pair_id] = false; } col_pair_contact_model_begin_id += this->m_max_num_contact_per_collision_pair; } #ifndef NDEBUG const std::size_t num_constraints = this->getNumberOfJointFrictionConstraints() // + this->getNumberOfBilateralConstraints() // + this->getNumberOfWeldConstraints() // + this->getNumberOfJointLimitConstraints() // + this->getNumberOfContacts(); assert(this->constraint_models.size() == num_constraints); assert(this->constraint_datas.size() == num_constraints); #endif // Call calc on the remaining constraint models (it has already been called for joint limits and frictional contacts) for (std::size_t i = 0; i < this->getNumberOfJointFrictionConstraints() + this->getNumberOfBilateralConstraints() + this->getNumberOfWeldConstraints(); ++i) { const ConstraintModel & constraint_model = this->constraint_models[i]; ConstraintData & constraint_data = this->constraint_datas[i]; constraint_model.calc(this->model(), this->data(), constraint_data); } // Update all constraints but frictional points constraints warmstarts if (this->joint_friction_constraint_size() > 0) { this->joint_friction_constraint_forces() = this->previous_joint_friction_constraint_forces(); } if (this->bilateral_constraints_size() > 0) { this->bilateral_constraints_forces() = this->previous_bilateral_constraints_forces(); } if (this->weld_constraints_size() > 0) { this->weld_constraints_forces() = this->previous_weld_constraints_forces(); } } // -------------------------------------------------------------------------- template class JointCollectionTpl> template void ConstraintsProblemTpl::build( const Eigen::MatrixBase & vfree, const Eigen::MatrixBase & v, const Eigen::MatrixBase & v_warmstart, S dt) { SIMPLE_TRACY_ZONE_SCOPED_N("ConstraintsProblem::build"); PINOCCHIO_UNUSED_VARIABLE(v_warmstart); PINOCCHIO_CHECK_ARGUMENT_SIZE(vfree.size(), v.size(), "The free velocity and the velocity should have the same size."); PINOCCHIO_CHECK_INPUT_ARGUMENT(dt >= 0, "The time step should be positive or null."); PINOCCHIO_CHECK_ARGUMENT_SIZE( this->constraint_models.size(), this->constraint_datas.size(), "The number of constraint models and datas should be the same."); assert(this->check()); // Compute `G`, the Delassus operator { SIMPLE_TRACY_ZONE_SCOPED_N("ConstraintsProblem::build - resize Delassus cholesky"); this->constraint_cholesky_decomposition.resize(this->model(), this->constraint_models); } { SIMPLE_TRACY_ZONE_SCOPED_N("ConstraintsProblem::build - compute Delassus cholesky"); // TODO(quentin): replace rho const Scalar rho = 1e-6; this->constraint_cholesky_decomposition.compute(this->model(), this->data(), this->constraint_models, this->constraint_datas, rho); } { SIMPLE_TRACY_ZONE_SCOPED_N("ConstraintsProblem::build - compute g"); this->computeConstraintsDrift(vfree, v, dt); } this->preconditioner().setOnes(); } // -------------------------------------------------------------------------- template class JointCollectionTpl> template void ConstraintsProblemTpl::computeConstraintsDrift( const Eigen::MatrixBase & vfree, const Eigen::MatrixBase & v, const Scalar dt) { // TODO: express everything in velocity and use pinocchio helper to go to formulation level int cindex = 0; for (std::size_t i = 0; i < this->constraint_models.size(); ++i) { const ConstraintModel & cmodel = this->constraint_models[i]; const ConstraintData & cdata = this->constraint_datas[i]; const int cdim = cmodel.activeSize(); auto gc = this->g().segment(cindex, cdim); using MapVectorXs = Eigen::Map; auto drift_visitor = ::simple::visitors::make_lambda_visitor( // // Visitor for FrictionalJointConstraintModel - velocity formulation [&](const FrictionalJointConstraintModel & cmodel) { const FrictionalJointConstraintData & cdata_ = boost::get(cdata); cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vfree, gc); }, // // Visitor for BilateralConstraintModel - velocity formulation [&](const BilateralPointConstraintModel & cmodel) { const BilateralPointConstraintData & cdata_ = boost::get(cdata); const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp; const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd; // -- Jc * (vfree - kd * v) MapVectorXs vtmp(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1)); vtmp = vfree - kd * v; cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmp, gc); // -- baumgarte kp correction const Vector3s correction = kp * cdata_.constraint_position_error; gc += correction / dt; }, // // Visitor for WeldConstraintModel - velocity formulation [&](const WeldConstraintModel & cmodel) { const WeldConstraintData & cdata_ = boost::get(cdata); const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp; const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd; // -- Jc * (vfree - kd * v) MapVectorXs vtmp(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1)); vtmp = vfree - kd * v; cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmp, gc); // -- baumgaarte kp correction const Vector6s correction = kp * cdata_.constraint_position_error; gc += correction / dt; }, // // Visitor for JointLimitConstraintModel - position formulation [&](const JointLimitConstraintModel & cmodel) { const JointLimitConstraintData & cdata_ = boost::get(cdata); const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp; const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd; // -- Jc * (vfree - kd * v) * dt MapVectorXs vtmpdt(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1)); vtmpdt = (vfree - kd * v) * dt; cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmpdt, gc); // -- baumgarte kp correction MapVectorXs joint_limit_correction(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, cmodel.activeSize(), 1)); joint_limit_correction = cdata_.constraint_residual; // copy const auto lower_bound_size = cmodel.set().getNegativeOrthant().size(); auto joint_limit_correction_lower = joint_limit_correction.head(lower_bound_size); joint_limit_correction_lower = (joint_limit_correction_lower.array() > 0) // if .select( // joint_limit_correction_lower.array() * kp, // then joint_limit_correction_lower.array()); // else const auto upper_bound_size = cmodel.set().getPositiveOrthant().size(); auto joint_limit_correction_upper = joint_limit_correction.tail(upper_bound_size); joint_limit_correction_upper = (joint_limit_correction_upper.array() < 0) // if .select( // joint_limit_correction_upper.array() * kp, // then joint_limit_correction_upper.array()); // else gc += joint_limit_correction; }, // // Visitor for FrictionalPointConstraintModel - velocity formulation [&](const FrictionalPointConstraintModel & cmodel) { const FrictionalPointConstraintData & cdata_ = boost::get(cdata); const Scalar kp = cmodel.baumgarte_corrector_parameters().Kp; const Scalar kd = cmodel.baumgarte_corrector_parameters().Kd; // TODO(quentin): no magic forces ? // TODO(quentin): take security_margin into account // TODO(louis): deal with elasticity. For that, we need to register which contact pairs were // in contact at the previous time step. // Vector3s correction(0, 0, std::min(0, Kb * penetration_depth / dt + elasticity * Jcv.coeff(2))); // -- Jc * (vfree - kd * v) MapVectorXs vtmp(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->model().nv, 1)); vtmp = vfree - kd * v; cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vtmp, gc); // -- baumgarte kp correction Vector3s correction = cdata_.constraint_position_error; correction = (correction.array() < 0) // if .select( // correction.array() * kp, // then 0.); // else gc += correction / dt; }); boost::apply_visitor(drift_visitor, cmodel); cindex += cdim; } } // -------------------------------------------------------------------------- template class JointCollectionTpl> void ConstraintsProblemTpl::updateConstraintsHolders() { this->m_current_constraints_pb_id = 1 - this->m_current_constraints_pb_id; } // -------------------------------------------------------------------------- template class JointCollectionTpl> bool ConstraintsProblemTpl::checkConstraintsConsistencyWithGeometryModel() const { assert( this->geom_data().collisionRequests.size() == this->geom_model().collisionPairs.size() // && "geom_data and geom_model do not coincide."); assert( this->geom_data().collisionRequests.size() == this->geom_data().collisionResults.size() // && "geom_data is inconsistent."); std::size_t max_num_contacts = this->geom_model().collisionPairs.size() * this->m_max_num_contact_per_collision_pair; return ( this->frictional_point_constraint_models.size() == max_num_contacts // && this->frictional_point_constraint_datas.size() == max_num_contacts); // } // -------------------------------------------------------------------------- template class JointCollectionTpl> void ConstraintsProblemTpl::collectActiveSet(Scalar epsilon) { // TODO we should add the active set for the other constraint types. this->breaking_contacts.clear(); this->sticking_contacts.clear(); this->sliding_contacts.clear(); this->contact_modes.clear(); const Vector3s e_z(0, 0, Scalar(1)); const auto nc = static_cast(this->getNumberOfContacts()); for (Eigen::Index contact_id = 0; contact_id < nc; ++contact_id) { const Vector3s lambda_i = this->frictional_point_constraints_forces().template segment<3>(3 * contact_id); // TODO(quentinll) check if the compliance is taken into account in sigma_i. It should be. const Vector3s sigma_i = this->frictional_point_constraints_velocities().template segment<3>(3 * contact_id); if (lambda_i.norm() < epsilon) // breaking { this->breaking_contacts.push_back((ContactIndex)contact_id); this->contact_modes.push_back(ContactMode::BREAKING); } else if (sigma_i.norm() < epsilon) // sticking { this->sticking_contacts.push_back((ContactIndex)contact_id); this->contact_modes.push_back(ContactMode::STICKING); } else // sliding { this->sliding_contacts.push_back((ContactIndex)contact_id); this->contact_modes.push_back(ContactMode::SLIDING); } } assert(this->contact_modes.size() == (std::size_t)nc && "Wrong size of contact modes vector."); } // -------------------------------------------------------------------------- template class JointCollectionTpl> bool ConstraintsProblemTpl::check() const { const auto problem_size = static_cast(this->constraints_problem_size()); assert(this->m_model != nullptr && "The model handle points to nullptr."); assert(this->m_data != nullptr && "The data handle points to nullptr."); assert(this->m_geom_model != nullptr && "The geometry model handle points to nullptr."); assert(this->m_geom_data != nullptr && "The geometry data handle points to nullptr."); const std::size_t num_constraints = this->getNumberOfJointFrictionConstraints() // + this->getNumberOfBilateralConstraints() // + this->getNumberOfWeldConstraints() // + this->getNumberOfJointLimitConstraints() // + this->getNumberOfContacts(); assert(this->constraint_models.size() == num_constraints); assert(this->constraint_datas.size() == num_constraints); assert( (this->m_g.size() == this->m_constraints_velocities_warmstarts.size()) // && "g and constraint should always have the same (maximum) size."); assert((this->m_g.size() >= problem_size) && "The size of m_g is too small to contain the contact problem."); assert((this->g().size() == problem_size) && "The size of g does not match the size of the contact problem."); assert( (this->frictional_point_constraints_warmstart().size() == this->frictional_point_constraints_size()) && "The size of the contact velocity warm-start does not match the size of the contact " "problem."); assert( (this->checkConstraintsConsistencyWithGeometryModel()) && "The constraints model and data vectors are not consistent with the geom_model and the " "geom_data."); return static_cast( this->m_model != nullptr // && this->m_data != nullptr // && this->m_geom_model != nullptr // && this->m_geom_data != nullptr // && this->constraint_models.size() == num_constraints // && this->constraint_datas.size() == num_constraints // && this->m_g.size() == this->m_constraints_velocities_warmstarts.size() // && this->m_g.size() >= problem_size // && this->g().size() == problem_size // && this->frictional_point_constraints_warmstart().size() == this->frictional_point_constraints_size() // && this->checkConstraintsConsistencyWithGeometryModel()); } } // namespace simple #endif // __simple_core_constraints_problem_hxx__ ================================================ FILE: include/simple/core/constraints-problem.txx ================================================ // // Copyright (c) 2022-2024 INRIA // #ifndef __simple_core_constraints_problem_txx__ #define __simple_core_constraints_problem_txx__ #include "simple/core/constraints-problem.hpp" namespace simple { extern template struct ConstraintsProblemTpl; } // namespace simple #endif // __simple_core_constraints_problem_txx__ ================================================ FILE: include/simple/core/contact-frame.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_contact_frame_hpp__ #define __simple_core_contact_frame_hpp__ #include "simple/core/fwd.hpp" #include #include #ifndef SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS #include #endif namespace simple { /// @brief Operator to construct a placement from a given normal and position. template struct PlacementFromNormalAndPositionTpl { using Scalar = _Scalar; enum { Options = _Options }; using SE3 = ::pinocchio::SE3Tpl; using Matrix6x3s = Eigen::Matrix; using Vector3s = Eigen::Matrix; /// \brief Computes a placement M from a normal and position. /// \param[in] normal is the z-axis of the rotation part of M. /// \param[in] position is the translation part of M. /// \param[out] M the placement. template static void calc( const Eigen::MatrixBase & normal, // [input] const Eigen::MatrixBase & position, // [input] SE3 & M // [output] ); /// \brief Computes the derivative of M w.r.t normal and position used to construct it. /// \param[in] M /// \param[out] dM_dnormal derivative w.r.t normal /// \param[out] dM_dposition derivative w.r.t position template static void calcDiff( const SE3 & M, // [input] const Eigen::MatrixBase & dM_dnormal, // [output] const Eigen::MatrixBase & dM_dposition // [output] ); protected: /// \brief Given a normal, returns the associated non-coliear reference vector. /// This vector is then used to compute the rotation part of the placement. template static Vector3s getReferenceVector(const Eigen::MatrixBase & normal) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3); Vector3s e_ref(PlacementFromNormalAndPosition::reference_vector()); static constexpr Scalar eps = 1e-6; if ((e_ref.cross(normal)).norm() <= eps) { e_ref = PlacementFromNormalAndPosition::other_reference_vector(); } return e_ref; } /// \brief Reference vector to compute the rotation part of a placement. /// If the normal is colinear to this reference vector, `e_ref2` is used instead. static Vector3s reference_vector() { return {1, 0, 0}; } /// \brief Other reference vector to compute the rotation part of a placement. static Vector3s other_reference_vector() { return {0, 1, 0}; } }; #ifndef SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS /// \brief Functor to compute the derivatives of a contact patch. struct ComputeContactPatchDerivative : ::diffcoal::ComputeContactPatchDerivative { using Base = ::diffcoal::ComputeContactPatchDerivative; using GeometryObject = ::pinocchio::GeometryObject; ComputeContactPatchDerivative(const GeometryObject & go1, const GeometryObject & go2) : Base(go1.geometry.get(), go2.geometry.get()) , go1_ptr(&go1) , go2_ptr(&go2) { } virtual ~ComputeContactPatchDerivative() {}; virtual void run( const ::hpp::fcl::Transform3f & M1, const ::hpp::fcl::Transform3f & M2, const ::hpp::fcl::ContactPatch & patch, const ::diffcoal::ContactPatchDerivativeRequest & drequest, ::diffcoal::ContactPatchDerivative & dpatch) const override { typedef ::hpp::fcl::CollisionGeometry const * Pointer; const_cast(Base::geom1) = go1_ptr->geometry.get(); const_cast(Base::geom2) = go2_ptr->geometry.get(); return Base::run(M1, M2, patch, drequest, dpatch); } bool operator==(const ComputeContactPatchDerivative & other) const { return Base::operator==(other) && go1_ptr == other.go1_ptr && go2_ptr == other.go2_ptr; // Maybe, it would be better to just check // *go2_ptr == *other.go2_ptr } bool operator!=(const ComputeContactPatchDerivative & other) const { return !(*this == other); } const GeometryObject & getGeometryObject1() const { return *go1_ptr; } const GeometryObject & getGeometryObject2() const { return *go2_ptr; } protected: const GeometryObject * go1_ptr; const GeometryObject * go2_ptr; }; #endif } // namespace simple #include "simple/core/contact-frame.hxx" #endif // __simple_core_contact_frame_hpp__ ================================================ FILE: include/simple/core/contact-frame.hxx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_contact_frame_hxx__ #define __simple_core_contact_frame_hxx__ namespace simple { // ========================================================================== template template void PlacementFromNormalAndPositionTpl::calc( const Eigen::MatrixBase & normal, // [input] const Eigen::MatrixBase & position, // [input] SE3 & M // [output] ) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Normal, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Position, 3); M.translation() = position; assert(normal.norm() - 1 <= 1e-12); // The normal will serve as the z-axis of M's rotation. const Vector3s normal_ = normal.normalized(); const Vector3s e_ref(PlacementFromNormalAndPosition::getReferenceVector(normal_)); M.rotation().col(2) = normal_; M.rotation().col(0) = (e_ref.cross(normal)).normalized(); M.rotation().col(1) = (M.rotation().col(2)).cross(M.rotation().col(0)); } // ======================================================================== template template void PlacementFromNormalAndPositionTpl::calcDiff( const SE3 & M, // [input] const Eigen::MatrixBase & dM_dnormal_, // [output] const Eigen::MatrixBase & dM_dposition_ // [output] ) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6x3Type1, 6, 3); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6x3Type1, 6, 3); Matrix6x3Type1 & dM_dnormal = const_cast(dM_dnormal_.derived()); Matrix6x3Type2 & dM_dposition = const_cast(dM_dposition_.derived()); // TODO(louis): exploit sparsity, don't use 6x3 matrices... // -> dM_dn linear part is always 0 // -> dM_dp angular part is always 0 /// dM_dnormal.setZero(); using Block3 = Eigen::Block; Block3 dM_dnormal_angular = dM_dnormal.template bottomRows<3>(); dM_dnormal_angular.row(0) = -M.rotation().col(1); dM_dnormal_angular.row(1) = M.rotation().col(0); const Vector3s e_ref(PlacementFromNormalAndPosition::getReferenceVector(M.rotation().col(2))); dM_dnormal_angular.row(2) = (M.rotation().col(1)).cross(e_ref) / (e_ref.cross(M.rotation().col(2))).norm(); /// dM_dposition.setZero(); dM_dposition.template topRows<3>() = M.rotation().transpose(); } } // namespace simple #endif // __simple_core_contact_frame_hxx__ ================================================ FILE: include/simple/core/fwd.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_fwd_hpp__ #define __simple_core_fwd_hpp__ #include "simple/fwd.hpp" #include namespace simple { namespace helper { using pinocchio::helper::get_pointer; using pinocchio::helper::get_ref; } // namespace helper template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl> struct ConstraintsProblemTpl; typedef ConstraintsProblemTpl ConstraintsProblem; template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl> struct SimulatorTpl; typedef SimulatorTpl Simulator; template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl> struct ContactSolverDerivativesTpl; typedef ContactSolverDerivativesTpl ContactSolverDerivatives; template class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl> struct SimulatorDerivativesTpl; typedef SimulatorDerivativesTpl SimulatorDerivatives; template struct PlacementFromNormalAndPositionTpl; typedef PlacementFromNormalAndPositionTpl PlacementFromNormalAndPosition; } // namespace simple #endif // ifndef __simple_core_fwd_hpp__ ================================================ FILE: include/simple/core/simulator.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_simulator_hpp__ #define __simple_core_simulator_hpp__ #include "simple/core/fwd.hpp" #include "simple/core/constraints-problem.hpp" #include #include #include #include #include #include #include #include namespace simple { template class JointCollectionTpl> struct traits> { using Scalar = _Scalar; }; template class JointCollectionTpl> struct SimulatorTpl // : ::pinocchio::NumericalBase> { // TODO: template by allocator EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Typedefs using Scalar = _Scalar; enum { Options = _Options }; using GeomIndex = pinocchio::GeomIndex; using VectorXs = Eigen::Matrix; using Vector3s = Eigen::Matrix; using Vector6s = Eigen::Matrix; using MatrixXs = Eigen::Matrix; using Model = ::pinocchio::ModelTpl; using ModelHandle = std::shared_ptr; using Data = typename Model::Data; using DataHandle = std::shared_ptr; using GeometryModel = ::pinocchio::GeometryModel; using GeometryModelHandle = std::shared_ptr; using GeometryData = ::pinocchio::GeometryData; using GeometryDataHandle = std::shared_ptr; using Force = typename Data::Force; using Motion = typename Data::Motion; using SE3 = typename Data::SE3; using JointModel = ::pinocchio::JointModelTpl; // TODO: template simulator by broad phase manager using BroadPhaseManager = ::pinocchio::BroadPhaseManagerTpl; using BroadPhaseManagerHandle = std::shared_ptr; using CollisionCallbackCollect = ::pinocchio::CollisionCallBackCollect; using ADMMConstraintSolver = ::pinocchio::ADMMContactSolverTpl; using PGSConstraintSolver = ::pinocchio::PGSContactSolverTpl; using SpatialForce = ::pinocchio::ForceTpl; using SpatialForceVector = PINOCCHIO_ALIGNED_STD_VECTOR(SpatialForce); using ConstraintsProblem = ConstraintsProblemTpl; using ConstraintsProblemHandle = std::shared_ptr; using ConstraintCholeskyDecomposition = typename ConstraintsProblem::ConstraintCholeskyDecomposition; using DelassusOperator = ::pinocchio::DelassusCholeskyExpressionTpl; using FrictionalPointConstraintModel = typename ConstraintsProblem::FrictionalPointConstraintModel; using FrictionalJointConstraintModel = typename ConstraintsProblem::FrictionalJointConstraintModel; using JointLimitConstraintModel = typename ConstraintsProblem::JointLimitConstraintModel; using BilateralPointConstraintModel = typename ConstraintsProblem::BilateralPointConstraintModel; using BilateralPointConstraintData = typename ConstraintsProblem::BilateralPointConstraintData; using BilateralPointConstraintModelVector = typename ConstraintsProblem::BilateralPointConstraintModelVector; using WeldConstraintModel = typename ConstraintsProblem::WeldConstraintModel; using WeldConstraintData = typename ConstraintsProblem::WeldConstraintData; using WeldConstraintModelVector = typename ConstraintsProblem::WeldConstraintModelVector; using ConstraintModel = typename ConstraintsProblem::ConstraintModel; using ConstraintData = typename ConstraintsProblem::ConstraintData; protected: /// \brief Handle to the model of the system. ModelHandle m_model; /// \brief Handle to the model's data of the system. DataHandle m_data; /// \brief Handle to the geometry model of the system. GeometryModelHandle m_geom_model; /// \brief Handle to the geometry model's data of the system. GeometryDataHandle m_geom_data; public: /// \brief Joints configuration of the system - copied from `step` input VectorXs q; /// \brief Joints velocity of the system - copied from `step` input VectorXs v; /// \brief External joint torques - copied from `step` input VectorXs tau; /// \brief External 6D force exerted in the local frame of each joint - copied from `step` input SpatialForceVector fext; /// \brief Time step of the simulator - copied from `step` input Scalar dt; /// \brief The updated joint configuration of the system. VectorXs qnew; /// \brief Free velocity i.e. the updated velocity of the system without any constraint forces. VectorXs vfree; /// \brief The updated velocity, taking into account the correction due to constraint forces. VectorXs vnew; /// \brief The updated acceleration, taking into account the correction due to constraint forces. VectorXs anew; /// \brief Vector of total spatial forces (i.e. external forces + constraint forces) applied on /// joints, expressed in the local frame of each joint. Note: by subtracting the external forces /// (given to the `step` method of the simulator) to `ftotal`, we get the constraint forces /// expressed in the local frame of the joints. SpatialForceVector ftotal; /// \brief Vector of total torques (i.e. applied tau + dry friction on joints + forces due to joint limits) applied on /// joints. VectorXs tau_total; /// \brief Vector of constraint torques VectorXs tau_constraints; /// \brief Base struct to set up constraint solvers struct ConstraintSolverSettings { int max_iter{1000}; Scalar absolute_precision{1e-8}; Scalar relative_precision{1e-8}; bool stat_record{false}; }; /// \brief Struct to store the settings for the ADMM constraint solver. /// /// Pinocchio's ADMM has two regularization term /// -> tau * rho for the augmented lagrangian term (consensus penalty) /// -> mu term for the proximal term (primal variable regularization) /// /// ADMM update rule: if ratio_primal_dual reached, rho *= rho_increment, where /// rho_increment = pow(L/m, rho_power_factor). /// /// Initialization of rho: /// -- If linear update rule -> rho is initialized by ADMM constructor or `setRho` /// -- If spectral update rule -> rho is initialized by ADMM `computeRho` = sqrt(L*m) * pow(L/m, rho_power) /// struct ADMMConstraintSolverSettings : ConstraintSolverSettings { Scalar mu{1e-6}; Scalar tau{0.5}; Scalar rho{10}; // initial value of rho for linear update rule Scalar rho_power{0.2}; // initial value of the rho_power for spectral update rule Scalar rho_power_factor{0.05}; Scalar linear_update_rule_factor{2}; Scalar ratio_primal_dual{50}; int lanczos_size{3}; // higher leads to more accurate eigvalues estimation. Max size = constraint problem size ::pinocchio::ADMMUpdateRule admm_update_rule{::pinocchio::ADMMUpdateRule::SPECTRAL}; }; /// \brief Settings for the ADMM constraint solver. ADMMConstraintSolverSettings admm_constraint_solver_settings; /// \brief Struct to store the settings for the PGS constraint solver. struct PGSConstraintSolverSettings : ConstraintSolverSettings { Scalar over_relax{1.0}; }; /// \brief Settings for the PGS constraint solver. PGSConstraintSolverSettings pgs_constraint_solver_settings; /// \brief Type of constraint solver used in the last call to the `step` method. /// This enum is meant to indicate which solver was used, not which one to use. /// To use a specific constraint solver, use the template of the `step` method. enum struct ConstraintSolverType { PGS, ADMM, NONE }; /// \brief Type of constraint solver used in the last call to the `step` method. ConstraintSolverType constraint_solver_type; /// \brief ADMM constraint solver. ADMMConstraintSolver admm_constraint_solver; /// \brief PGS constraint solver. PGSConstraintSolver pgs_constraint_solver; /// \brief Whether or not to warm-start the constraint solver. bool warm_start_constraints_forces{true}; /// \brief Whether or not timings of the `step` function are measured. /// If set to true, the timing of the last call to `step` can be accessed via `getCPUTimes` bool measure_timings{false}; /// \brief Get timings of the last call to the `step` method. hpp::fcl::CPUTimes getStepCPUTimes() const { return this->m_step_timings; } /// \brief Get timings of the call to the constraint solver in the last call of `step`. /// This timing is set to 0 if there was no constraints. hpp::fcl::CPUTimes getConstraintSolverCPUTimes() const { return this->m_constraint_solver_timings; } /// \brief Get timings of the collision detection stage. hpp::fcl::CPUTimes getCollisionDetectionCPUTimes() const { return this->m_collision_detection_timings; } /// \brief Virtual destructor of the base class virtual ~SimulatorTpl() { } protected: /// \brief Broad phase manager // TODO(louis): template by broad phase type (no broad phase, dynamic aabb tree, SAP etc) BroadPhaseManagerHandle m_broad_phase_manager; /// \brief Collision callback CollisionCallbackCollect m_collision_callback; /// \brief Constraint problem ConstraintsProblemHandle m_constraints_problem; /// \brief Temporary variable used to integrate the system's state VectorXs m_vnew_integration_tmp; /// \brief Whether or not the simulator has been reset. bool m_is_reset; /// \brief Timer used for the whole `step` method hpp::fcl::Timer m_step_timer; /// \brief Timings for the whole `step` method. hpp::fcl::CPUTimes m_step_timings; /// \brief Timer used for internal methods inside `step`. hpp::fcl::Timer m_internal_timer; /// \brief Timings for the call to the constraint solver. hpp::fcl::CPUTimes m_constraint_solver_timings; /// \brief Timings for collision detection. hpp::fcl::CPUTimes m_collision_detection_timings; public: /// /// \brief Constructor specifying the Data and GeometryData associated to the model and /// geom_model. SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models, const WeldConstraintModelVector & weld_constraint_models); /// /// \brief Constructor using model, data, geometry model, geometry data and vector of bilateral constraints. SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models); /// /// \brief Constructor specifying the Data and GeometryData associated to the model and /// geom_model. SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle, const WeldConstraintModelVector & weld_constraint_models); /// /// \brief Constructor specifying the Data and GeometryData associated to the model and /// geom_model. SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle); /// /// \brief Default constructor /// Whenever the model or the geometry model is changed, this constructor should be called. SimulatorTpl( ModelHandle model_handle, GeometryModelHandle geom_model_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models, const WeldConstraintModelVector & weld_constraint_models); /// /// \brief Default constructor /// Whenever the model or the geometry model is changed, this constructor should be called. SimulatorTpl( ModelHandle model_handle, GeometryModelHandle geom_model_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models); /// /// \brief Default constructor /// Whenever the model or the geometry model is changed, this constructor should be called. SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle, const WeldConstraintModelVector & weld_constraint_models); /// /// \brief Default constructor /// Whenever the model or the geometry model is changed, this constructor should be called. SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle); /// /// \brief Resets the internal quantities of the simulator. /// This methods needs to be called before looping on the `step` method, for example when the /// initial state (q0, v0) of the system is used as an input to `step` and (q0, v0) have not /// been computed using the `step` method. \note If, instead, the simulator's model, data, /// geom_model or geom_data have changed, please call the constructor. void reset(); /// /// \brief Main step function of the simulator. template< template class ConstraintSolver = ::pinocchio::ADMMContactSolverTpl, typename ConfigVectorType, typename VelocityVectorType, typename TorqueVectorType> void step( const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & tau, Scalar dt); /// /// \brief Main step function of the simulator. /// fext must be expressed in the local frame of each joint. // TODO: remove aligned_vector and template by Allocator. // TODO: change MatrixBase to PlainObject template< template class ConstraintSolver = ::pinocchio::ADMMContactSolverTpl, typename ConfigVectorType, typename VelocityVectorType, typename TorqueVectorType, typename ForceDerived> void step( const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & tau, const pinocchio::container::aligned_vector & fext, const Scalar dt); /// /// \brief Returns true if the simulator is in its reset state. bool isReset() const { return this->m_is_reset; } /// /// \brief Check consistency of the simulator. bool check() const; /// /// \brief Check consistency of the collision pairs. /// This method checks thath the geometry model does not contain collision pairs between /// geometry objects from the same parent joint. bool checkCollisionPairs() const; /// /// \brief Returns a const reference to the model const Model & model() const { return pinocchio::helper::get_ref(this->m_model); } /// /// \brief Returns a reference to the model Model & model() { return pinocchio::helper::get_ref(this->m_model); } /// /// \brief Returns a const reference to the data const Data & data() const { return pinocchio::helper::get_ref(this->m_data); } /// /// \brief Returns a reference to the data Data & data() { return pinocchio::helper::get_ref(this->m_data); } /// /// \brief Returns a const reference to the geometry model const pinocchio::GeometryModel & geom_model() const { return pinocchio::helper::get_ref(this->m_geom_model); } /// /// \brief Returns a reference to the geometry model pinocchio::GeometryModel & geom_model() { return pinocchio::helper::get_ref(this->m_geom_model); } /// /// \brief Returns a const reference to the geometry data const pinocchio::GeometryData & geom_data() const { return pinocchio::helper::get_ref(this->m_geom_data); } /// /// \brief Returns a reference to the geometry data pinocchio::GeometryData & geom_data() { return pinocchio::helper::get_ref(this->m_geom_data); } /// /// \brief Returns a const reference to the constraint problem const ConstraintsProblem & constraints_problem() const { return pinocchio::helper::get_ref(this->m_constraints_problem); } /// /// \brief Returns a const reference to the constraint problem ConstraintsProblem & constraints_problem() { return pinocchio::helper::get_ref(this->m_constraints_problem); } /// /// \brief Returns a handle to the constraint problem ConstraintsProblemHandle getConstraintsProblemHandle() const { return this->m_constraints_problem; } protected: /// /// \brief Allocates memory based on `model` and active collision pairs in `geom_model`. void allocate(); /// /// \brief Initializes the geometry data for broad and narrow phase collision detection. void initializeGeometryData(); /// /// \brief Warm starting constraint forces via constraint inverse dynamics. void warmStartConstraintForces(); /// /// \brief Collision detection void detectCollisions(); /// /// \brief Preambule function meant to be run before resolving collisions. virtual void preambleResolveConstraints(const Scalar dt) { PINOCCHIO_UNUSED_VARIABLE(dt); } /// /// \brief Constraint resolution template class ConstraintSolver, typename VelocityVectorType> void resolveConstraints(const Eigen::MatrixBase & v, const Scalar dt); }; namespace details { template class SolverTpl, typename _Scalar, int _Options, template class JointCollectionTpl> struct SimulatorConstraintSolverTpl { }; template class JointCollectionTpl> struct SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl> { using Scalar = _Scalar; enum { Options = _Options }; using Simulator = SimulatorTpl; using ConstraintsProblem = typename Simulator::ConstraintsProblem; using ADMMConstraintSolverSettings = typename Simulator::ADMMConstraintSolverSettings; using ADMMConstraintSolver = ::pinocchio::ADMMContactSolverTpl; using MatrixXs = typename Simulator::MatrixXs; using VectorXs = typename Simulator::VectorXs; using RefConstVectorXs = Eigen::Ref; static void run(Simulator & simulator, Scalar dt); protected: static void setup(Simulator & simulator); }; template class JointCollectionTpl> struct SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl> { using Scalar = _Scalar; enum { Options = _Options }; using Simulator = SimulatorTpl; using ConstraintsProblem = typename Simulator::ConstraintsProblem; using PGSConstraintSolverSettings = typename Simulator::PGSConstraintSolverSettings; using PGSConstraintSolver = ::pinocchio::PGSContactSolverTpl; using DelassusOperatorDense = ::pinocchio::DelassusOperatorDenseTpl; using MatrixXs = typename Simulator::MatrixXs; using VectorXs = typename Simulator::VectorXs; using RefConstVectorXs = Eigen::Ref; static void run(Simulator & simulator, Scalar dt); protected: static void setup(Simulator & simulator); }; } // namespace details } // namespace simple #include "simple/core/simulator.hxx" #if SIMPLE_ENABLE_TEMPLATE_INSTANTIATION #include "simple/core/simulator.txx" #include "simple/pinocchio_template_instantiation/aba.txx" #include "simple/pinocchio_template_instantiation/joint-model.txx" #include "simple/pinocchio_template_instantiation/crba.txx" #endif #endif // ifndef __simple_core_simulator_hpp__ ================================================ FILE: include/simple/core/simulator.hxx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_core_simulator_hxx__ #define __simple_core_simulator_hxx__ #include #include #include #include #include #include #include #include #include #include #include "simple/tracy.hpp" // #include #include #include #include namespace simple { // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models, const WeldConstraintModelVector & weld_constraint_models) : m_model(model_handle) , m_data(data_handle) , m_geom_model(geom_model_handle) , m_geom_data(geom_data_handle) , q(this->model().nq) , v(this->model().nv) , tau(this->model().nv) , fext(static_cast(this->model().njoints), Force::Zero()) , dt(-1) , qnew(this->model().nq) , vfree(this->model().nv) , vnew(this->model().nv) , anew(this->model().nv) , ftotal(static_cast(this->model().njoints), Force::Zero()) , tau_total(this->model().nv) , tau_constraints(this->model().nv) , constraint_solver_type(ConstraintSolverType::NONE) , admm_constraint_solver(0) // only create it when needed , pgs_constraint_solver(0) // only create it when needed , m_broad_phase_manager( new BroadPhaseManager(&helper::get_ref(model_handle), &helper::get_ref(geom_model_handle), &helper::get_ref(geom_data_handle))) , m_collision_callback(this->geom_model(), this->geom_data()) , m_constraints_problem(new ConstraintsProblem( model_handle, data_handle, geom_model_handle, geom_data_handle, bilateral_constraint_models, weld_constraint_models)) , m_vnew_integration_tmp(this->model().nv) , m_is_reset(true) , m_step_timer(false) , m_internal_timer(false) { this->initializeGeometryData(); this->allocate(); } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models) : SimulatorTpl(model_handle, data_handle, geom_model_handle, geom_data_handle, bilateral_constraint_models, WeldConstraintModelVector()) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle, const WeldConstraintModelVector & weld_constraint_models) : SimulatorTpl( model_handle, data_handle, geom_model_handle, geom_data_handle, BilateralPointConstraintModelVector(), weld_constraint_models) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, DataHandle data_handle, GeometryModelHandle geom_model_handle, GeometryDataHandle geom_data_handle) : SimulatorTpl( model_handle, data_handle, geom_model_handle, geom_data_handle, BilateralPointConstraintModelVector(), WeldConstraintModelVector()) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, GeometryModelHandle geom_model_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models, const WeldConstraintModelVector & weld_constraint_models) : SimulatorTpl( model_handle, std::make_shared<::pinocchio::Data>(*model_handle), geom_model_handle, std::make_shared<::pinocchio::GeometryData>(*geom_model_handle), bilateral_constraint_models, weld_constraint_models) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, GeometryModelHandle geom_model_handle, const BilateralPointConstraintModelVector & bilateral_constraint_models) : SimulatorTpl(model_handle, geom_model_handle, bilateral_constraint_models, WeldConstraintModelVector()) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl( ModelHandle model_handle, GeometryModelHandle geom_model_handle, const WeldConstraintModelVector & weld_constraint_models) : SimulatorTpl(model_handle, geom_model_handle, BilateralPointConstraintModelVector(), weld_constraint_models) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> SimulatorTpl::SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle) : SimulatorTpl(model_handle, geom_model_handle, BilateralPointConstraintModelVector(), WeldConstraintModelVector()) { } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorTpl::allocate() { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::allocate"); this->constraints_problem().allocate(); // this->q.resize(this->model().nq); this->v.resize(this->model().nv); this->tau.resize(this->model().nv); this->fext.resize(static_cast(model().njoints), Force::Zero()); // this->qnew.resize(this->model().nq); this->vfree.resize(this->model().nv); this->vnew.resize(this->model().nv); this->anew.resize(this->model().nv); this->anew.setZero(); this->ftotal.resize(static_cast(model().njoints), Force::Zero()); this->tau_total.resize(this->model().nv); this->tau_total.setZero(); this->tau_constraints.resize(this->model().nv); this->tau_constraints.setZero(); this->m_vnew_integration_tmp.resize(this->model().nv); } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorTpl::initializeGeometryData() { // Broad phase setup for (::pinocchio::GeometryObject & geom : this->geom_model().geometryObjects) { geom.geometry->computeLocalAABB(); } // Narrow phase setup for (hpp::fcl::CollisionRequest & request : this->geom_data().collisionRequests) { request.enable_contact = true; } } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorTpl::reset() { // Reset constraint problem this->constraints_problem().clear(); this->constraints_problem().constraints_forces().setZero(); this->constraints_problem().previous_constraints_forces().setZero(); // Reset constraint solver this->constraint_solver_type = ConstraintSolverType::NONE; // Reset timings this->m_step_timings.clear(); this->m_constraint_solver_timings.clear(); this->m_is_reset = true; } // -------------------------------------------------------------------------- template class JointCollectionTpl> template class ConstraintSolver, typename ConfigVectorType, typename VelocityVectorType, typename TorqueVectorType> void SimulatorTpl::step( const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & tau, Scalar dt) { this->fext.assign((std::size_t)(model().njoints), Force::Zero()); this->step(q, v, tau, this->fext, dt); } // -------------------------------------------------------------------------- template class JointCollectionTpl> template< template class ConstraintSolver, typename ConfigVectorType, typename VelocityVectorType, typename TorqueVectorType, typename ForceDerived> void SimulatorTpl::step( const Eigen::MatrixBase & q_, const Eigen::MatrixBase & v_, const Eigen::MatrixBase & tau_, const ::pinocchio::container::aligned_vector & fext_, const Scalar dt_) { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::step"); // clang-format off if (this->measure_timings) { this->m_step_timer.start(); }; // clang-format on // TODO(louis): should we use check arguments or assert/throw? PINOCCHIO_CHECK_ARGUMENT_SIZE( this->vfree.size(), this->model().nv, "The sizes of the free velocity of the simulator and the input velocity do not match. " "You problably changed your model, data, geom_model or geom_data and forgot to call " "allocate()."); PINOCCHIO_CHECK_ARGUMENT_SIZE(q_.size(), this->model().nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v_.size(), this->model().nv, "The joint velocity vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(tau_.size(), this->model().nv, "The joint torque vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( fext_.size(), static_cast(this->model().njoints), "The external forces vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT(dt_ >= 0, "dt is not >= 0"); assert(this->check() && "The simulator is not properly instanciated."); // Record state of simulator this->q = q_; this->v = v_; this->tau = tau_; this->fext = fext_; this->dt = dt_; // Set up data for downstream algorithms this->data().q_in = q; this->data().v_in = v; this->data().tau_in = tau; // Compute the mass matrix of the system - used by the delassus operator { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::step - compute crba"); ::pinocchio::crba(this->model(), this->data(), q, pinocchio::Convention::WORLD); } // Compute free acceleration of the system { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::step - compute vfree (first aba)"); this->ftotal = fext; this->tau_total = tau; // clang-format off this->vfree = v + dt * ::pinocchio::aba(this->model(), this->data(), q, v, this->tau_total, this->ftotal, pinocchio::Convention::WORLD); // clang-format on } // Collision detection this->detectCollisions(); // Update constraint problem with result of collision detection const bool compute_constraints_forces_warm_start = this->warm_start_constraints_forces && (!this->isReset()); this->constraints_problem().update(compute_constraints_forces_warm_start); this->tau_constraints.setZero(); if (this->constraints_problem().constraints_problem_size() > 0) { // Constraint resolution - compute the total joint forces (if there are any collisions) i.e. external forces + constraint forces this->resolveConstraints(v, dt); { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::step - compute vnew (second aba)"); this->anew = ::pinocchio::aba(this->model(), this->data(), q, v, this->tau_total, this->ftotal, pinocchio::Convention::WORLD); this->vnew = v + dt * this->anew; } } else { this->anew = (this->vfree - v) / dt; // TODO(quentin) do it differrently this->vnew = this->vfree; // Reset constraint solver timings this->m_constraint_solver_timings.clear(); } this->m_vnew_integration_tmp = this->vnew * dt; ::pinocchio::integrate(this->model(), q, this->m_vnew_integration_tmp, this->qnew); this->m_is_reset = false; // clang-format off if (this->measure_timings) { this->m_step_timer.stop(); }; this->m_step_timings = this->m_step_timer.elapsed(); // clang-format on } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorTpl::detectCollisions() { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::detectCollisions"); // clang-format off if (this->measure_timings) { this->m_internal_timer.start(); } // clang-format on // Compute oMg for each geometry ::pinocchio::updateGeometryPlacements(this->model(), this->data(), this->geom_model(), this->geom_data()); // Reset collision results - super important! Otherwise constraint from previous time step may be // detected between non-colliding geometries. for (hpp::fcl::CollisionResult & col_res : this->geom_data().collisionResults) { col_res.clear(); } // Run broad + narrow phase collision detection const bool recompute_local_aabb = false; // already computed in `initializeGeometryData` this->m_broad_phase_manager->update(recompute_local_aabb); assert(this->m_broad_phase_manager->check() && "The broad phase manager is not aligned with the geometry model."); // --> Broad Phase { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::detectCollisions - Broad phase"); ::pinocchio::computeCollisions(*(this->m_broad_phase_manager), &this->m_collision_callback); } // --> Narrow Phase { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::detectCollisions - Narrow phase"); for (std::size_t i = 0; i < this->m_collision_callback.pair_indexes.size(); ++i) { const std::size_t pair_index = this->m_collision_callback.pair_indexes[i]; const ::pinocchio::CollisionPair & cp = this->geom_model().collisionPairs[pair_index]; const ::pinocchio::GeometryObject & obj1 = this->geom_model().geometryObjects[cp.first]; const ::pinocchio::GeometryObject & obj2 = this->geom_model().geometryObjects[cp.second]; // middle phase collision detection coal::CollisionRequest collision_request(this->geom_data().collisionRequests[pair_index]); bool obb_overlap = false; if ( obj1.geometry->getNodeType() == coal::GEOM_PLANE || obj1.geometry->getNodeType() == coal::GEOM_HALFSPACE || obj2.geometry->getNodeType() == coal::GEOM_PLANE || obj2.geometry->getNodeType() == coal::GEOM_HALFSPACE) { obb_overlap = true; } else { coal::Transform3s oM1(toFclTransform3f(this->geom_data().oMg[cp.first])); coal::Transform3s oM2(toFclTransform3f(this->geom_data().oMg[cp.second])); const Scalar security_margin = collision_request.security_margin; // const coal::AABB aabb1 = obj1.geometry->aabb_local.expand(security_margin * 0.5); coal::OBB obb1; coal::convertBV(aabb1, oM1, obb1); // const coal::AABB aabb2 = obj1.geometry->aabb_local.expand(security_margin * 0.5); coal::OBB obb2; coal::convertBV(aabb2, oM2, obb2); obb_overlap = obb1.overlap(obb2); } try { if (obb_overlap) { pinocchio::computeCollision(this->geom_model(), this->geom_data(), pair_index, collision_request); ::pinocchio::computeContactPatch(this->geom_model(), this->geom_data(), pair_index); } } catch (std::logic_error & e) { PINOCCHIO_THROW_PRETTY( std::logic_error, "Geometries with index go1: " << cp.first << " or go2: " << cp.second << " have produced an internal error within Coal.\n what:\n" << e.what()); } } } // clang-format off if (this->measure_timings) { this->m_internal_timer.start(); } this->m_collision_detection_timings = this->m_internal_timer.elapsed(); // clang-format on } // -------------------------------------------------------------------------- template class JointCollectionTpl> template class ConstraintSolver, typename VelocityVectorType> void SimulatorTpl::resolveConstraints(const Eigen::MatrixBase & v, const Scalar dt) { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::resolveConstraints"); assert( !this->constraints_problem().constraint_models.empty() && "Resolve collisions should not be called if there are no constraints."); // Build the constraint quantities for the constraint solver // TODO(quentin): warm-start constraint forces via constraint inverse dynamics this->constraints_problem().build(this->vfree, v, v + dt * this->anew, dt); this->preambleResolveConstraints(dt); // Call the constraint solver // clang-format off if (this->measure_timings) { this->m_internal_timer.start(); }; details::SimulatorConstraintSolverTpl::run(*this, dt); if (this->measure_timings) { this->m_internal_timer.stop(); }; this->m_constraint_solver_timings = this->m_internal_timer.elapsed(); // clang-format on { SIMPLE_TRACY_ZONE_SCOPED_N("Simulator::resolveConstraints - apply constraint forces"); ::pinocchio::evalConstraintJacobianTransposeMatrixProduct( this->model(), this->data(), // this->constraints_problem().constraint_models, // this->constraints_problem().constraint_datas, // this->constraints_problem().constraints_forces(), // this->tau_constraints); this->tau_total += this->tau_constraints; } } // -------------------------------------------------------------------------- template class JointCollectionTpl> bool SimulatorTpl::checkCollisionPairs() const { for (GeomIndex col_pair_id = 0; col_pair_id < this->geom_model().collisionPairs.size(); col_pair_id++) { const GeomIndex geom_id1 = this->geom_model().collisionPairs[col_pair_id].first; const GeomIndex geom_id2 = this->geom_model().collisionPairs[col_pair_id].second; const ::pinocchio::GeometryObject & geom1 = this->geom_model().geometryObjects[geom_id1]; const ::pinocchio::GeometryObject & geom2 = this->geom_model().geometryObjects[geom_id2]; if (geom1.parentJoint == geom2.parentJoint) { return false; } } return true; } // -------------------------------------------------------------------------- template class JointCollectionTpl> bool SimulatorTpl::check() const { assert(this->m_model != nullptr && "The model handle points to nullptr."); assert(this->m_data != nullptr && "The data handle points to nullptr."); assert(this->m_geom_model != nullptr && "The geometry model handle points to nullptr."); assert(this->m_geom_data != nullptr && "The geometry data handle points to nullptr."); assert(this->m_broad_phase_manager->check() && "The broad phase manager is not aligned with the geometry model."); assert(this->vfree.size() == this->model().nv && "The free velocity vector is not of right size."); assert(this->vnew.size() == this->model().nv && "The new velocity vector is not of right size."); assert(this->ftotal.size() == static_cast(this->model().njoints) && "The total force vector is not of right size."); assert(this->constraints_problem().check() && "The constraint problem is invalid."); assert( this->checkCollisionPairs() && "The GeometryModel contains collision pairs between GeometryObjects attached to the same " "joint."); return static_cast( this->m_model && this->m_data // && this->m_geom_model // && this->m_geom_data // && this->m_broad_phase_manager->check() // && this->vfree.size() == this->model().nv // && this->vnew.size() == this->model().nv // && this->ftotal.size() == static_cast(model().njoints) // && this->constraints_problem().check() // && this->checkCollisionPairs()); } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorTpl::warmStartConstraintForces() { boost::optional> lambda_guess(this->constraints_problem().constraints_forces()); // ::pinocchio::computeContactForces( // this->model(), this->data(), this->constraints_problem().constraints_velocities_warmstarts(), // this->constraints_problem().constraint_models, this->constraints_problem().constraint_datas, // this->constraints_problem().contact_compliances(), this->contact_solver_info, lambda_guess); const auto problem_size = static_cast(this->constraints_problem().constraints_problem_size()); this->constraints_problem().constraints_forces() = (this->data().lambda_c.head(problem_size)); } namespace details { // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::setup(Simulator & simulator) { SIMPLE_TRACY_ZONE_SCOPED_N("SimulatorConstraintSolver::setup"); ADMMConstraintSolver & solver = simulator.admm_constraint_solver; const ADMMConstraintSolverSettings & settings = simulator.admm_constraint_solver_settings; // TODO: should we reuse rho and rho_power or always reset them? const Scalar mu = settings.mu; const Scalar tau = settings.tau; const Scalar rho = settings.rho; const Scalar rho_power = settings.rho_power; const Scalar rho_power_factor = settings.rho_power_factor; const Scalar linear_update_rule_factor = settings.linear_update_rule_factor; const Scalar ratio_primal_dual = settings.ratio_primal_dual; const int lanczos_size = settings.lanczos_size; const auto problem_size = static_cast(simulator.constraints_problem().constraints_problem_size()); if ( simulator.isReset() || simulator.constraint_solver_type != Simulator::ConstraintSolverType::ADMM || solver.getPrimalSolution().size() != problem_size) { solver = ADMMConstraintSolver( problem_size, mu, tau, rho_power, rho_power_factor, // linear_update_rule_factor, ratio_primal_dual, lanczos_size); solver.setRho(rho); } else { solver.setProximalValue(mu); solver.setTau(tau); solver.setRho(rho); solver.setRhoPower(rho_power); solver.setRhoPowerFactor(rho_power_factor); solver.setLinearUpdateRuleFactor(linear_update_rule_factor); solver.setRatioPrimalDual(ratio_primal_dual); solver.setLanczosSize(lanczos_size); } simulator.constraint_solver_type = Simulator::ConstraintSolverType::ADMM; solver.setMaxIterations(settings.max_iter); solver.setAbsolutePrecision(settings.absolute_precision); solver.setRelativePrecision(settings.relative_precision); } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::run( Simulator & simulator, Scalar dt) { SIMPLE_TRACY_ZONE_SCOPED_N("SimulatorConstraintSolver::run"); ConstraintsProblem & constraints_problem = simulator.constraints_problem(); ADMMConstraintSolver & solver = simulator.admm_constraint_solver; const ADMMConstraintSolverSettings & settings = simulator.admm_constraint_solver_settings; // Create/update constraint solver setup(simulator); PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED(); // Delassus typename Simulator::DelassusOperator G(constraints_problem.constraint_cholesky_decomposition); // Drift term auto g = constraints_problem.g(); // Preconditioner auto preconditioner = constraints_problem.preconditioner(); // Input/output of the solver auto constraints_forces = constraints_problem.constraints_forces(); if (!simulator.warm_start_constraints_forces) { constraints_forces.setZero(); } solver.solve( G, g, constraints_problem.constraint_models, dt, boost::make_optional((RefConstVectorXs)preconditioner), // boost::make_optional((RefConstVectorXs)constraints_forces), boost::none, constraints_problem.is_ncp, settings.admm_update_rule, settings.stat_record); // Get solution of the solver constraints_forces = solver.getPrimalSolution(); // Get constraint velocities -> TODO: it's no velocities anymore constraints_problem.constraints_velocities() = solver.getDualSolution(); if (simulator.constraints_problem().is_ncp) { constraints_problem.constraints_velocities() += -solver.getComplementarityShift(); } // Get time scaling for derivatives ::pinocchio::internal::getTimeScalingFromAccelerationToConstraints( constraints_problem.constraint_models, dt, constraints_problem.time_scaling_acc_to_constraints()); PINOCCHIO_EIGEN_MALLOC_ALLOWED(); } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::setup(Simulator & simulator) { SIMPLE_TRACY_ZONE_SCOPED_N("SimulatorConstraintSolver::setup"); PGSConstraintSolver & solver = simulator.pgs_constraint_solver; const PGSConstraintSolverSettings & settings = simulator.pgs_constraint_solver_settings; const auto problem_size = static_cast(simulator.constraints_problem().constraints_problem_size()); if ( simulator.isReset() || simulator.constraint_solver_type != Simulator::ConstraintSolverType::PGS || solver.getPrimalSolution().size() != problem_size) { solver = PGSConstraintSolver(problem_size); } simulator.constraint_solver_type = Simulator::ConstraintSolverType::PGS; solver.setMaxIterations(settings.max_iter); solver.setAbsolutePrecision(settings.absolute_precision); solver.setRelativePrecision(settings.relative_precision); } // -------------------------------------------------------------------------- template class JointCollectionTpl> void SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl>::run( Simulator & simulator, Scalar dt) { SIMPLE_TRACY_ZONE_SCOPED_N("SimulatorConstraintSolver::run"); ConstraintsProblem & constraints_problem = simulator.constraints_problem(); PGSConstraintSolver & solver = simulator.pgs_constraint_solver; const PGSConstraintSolverSettings & settings = simulator.pgs_constraint_solver_settings; // Create constraint solver setup(simulator); // Delassus typename Simulator::DelassusOperator G(constraints_problem.constraint_cholesky_decomposition); const bool enforce_symmetry = true; const DelassusOperatorDense delassus_dense(G, enforce_symmetry); // Drift term auto g = constraints_problem.g(); // Warm-start and solution of the constraint solver auto constraints_forces = constraints_problem.constraints_forces(); if (!simulator.warm_start_constraints_forces) { constraints_forces.setZero(); } solver.solve( delassus_dense, g, constraints_problem.constraint_models, dt, boost::make_optional((RefConstVectorXs)constraints_forces), // settings.over_relax, constraints_problem.is_ncp, settings.stat_record); // Get solution of the solver constraints_forces = solver.getPrimalSolution(); // Get constraint velocities constraints_problem.constraints_velocities() = solver.getDualSolution(); // Get time scaling for derivatives ::pinocchio::internal::getTimeScalingFromAccelerationToConstraints( constraints_problem.constraint_models, dt, constraints_problem.time_scaling_acc_to_constraints()); } } // namespace details } // namespace simple #endif // __simple_core_simulator_hxx__ ================================================ FILE: include/simple/core/simulator.txx ================================================ // // Copyright (c) 2022-2024 INRIA // #ifndef __simple_simulator_txx__ #define __simple_simulator_txx__ #include "simple/core/simulator.hpp" namespace simple { extern template struct SimulatorTpl; extern template void SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, context::Scalar); extern template void SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &, context::Scalar); extern template void SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, context::Scalar); extern template void SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &, context::Scalar); extern template void SimulatorTpl::resolveConstraints< ::pinocchio::ADMMContactSolverTpl>(const Eigen::MatrixBase &, const Scalar); extern template void SimulatorTpl::resolveConstraints< ::pinocchio::PGSContactSolverTpl>(const Eigen::MatrixBase &, const Scalar); } // namespace simple #endif // __simple_simulator_txx__ ================================================ FILE: include/simple/fwd.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_fwd_hpp__ #define __simple_fwd_hpp__ #ifdef _WIN32 #include #undef far #undef near #endif #include #ifdef SIMPLE_EIGEN_CHECK_MALLOC #ifndef EIGEN_RUNTIME_NO_MALLOC #define EIGEN_RUNTIME_NO_MALLOC_WAS_NOT_DEFINED #define EIGEN_RUNTIME_NO_MALLOC #endif #endif // #include "simple/macros.hpp" #include "simple/deprecated.hpp" #include "simple/warning.hpp" #include "simple/config.hpp" // Include Eigen components #include #include #include #ifdef SIMPLE_WITH_ACCELERATE_SUPPORT #include #endif #include namespace simple { /// /// \brief Common traits structure to fully define base classes for CRTP. /// template struct traits { }; namespace context { typedef double Scalar; enum { Options = 0 }; // Common eigen types using Vector2s = Eigen::Matrix; using Vector3s = Eigen::Matrix; using Vector6s = Eigen::Matrix; using Matrix6s = Eigen::Matrix; using Matrix63s = Eigen::Matrix; using Matrix6Xs = Eigen::Matrix; using VectorXs = Eigen::Matrix; using MatrixXs = Eigen::Matrix; using RowMatrixXs = Eigen::Matrix; // Commong pinocchio types using Force = ::pinocchio::ForceTpl; using Motion = ::pinocchio::MotionTpl; using SE3 = ::pinocchio::SE3Tpl; using Model = ::pinocchio::ModelTpl; using Data = ::pinocchio::DataTpl; } // namespace context } // namespace simple #endif // ifndef __simple_fwd_hpp__ ================================================ FILE: include/simple/math/fwd.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_math_fwd_hpp__ #define __simple_math_fwd_hpp__ #include "simple/fwd.hpp" namespace simple { } // namespace simple #endif // ifndef __simple_math_fwd_hpp__ ================================================ FILE: include/simple/math/qr.hpp ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_math_qr_hpp__ #define __simple_math_qr_hpp__ #include "simple/math/fwd.hpp" #include namespace simple { namespace math { template struct SolveInPlaceWrapper : _SolverType { typedef _SolverType SolverType; template void solveInPlace(const Eigen::MatrixBase & mat) const { typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixType) res(mat); res = this->solve(mat); mat.const_cast_derived() = res; } }; // struct SolveInPlaceWrapper template struct SolveInPlaceWrapper> : Eigen::HouseholderQR<_MatrixType> { typedef Eigen::HouseholderQR<_MatrixType> SolverType; template void solveInPlace(const Eigen::MatrixBase & mat_) const { const Eigen::Index rank = (std::min)(this->rows(), this->cols()); MatrixType & mat = mat_.const_cast_derived(); mat.applyOnTheLeft(this->householderQ().setLength(rank).adjoint()); this->m_qr.topLeftCorner(rank, rank).template triangularView().solveInPlace(mat.topRows(rank)); mat.bottomRows(this->cols() - rank).setZero(); } }; } // namespace math } // namespace simple #endif // ifndef __simple_math_qr_hpp__ ================================================ FILE: include/simple/pch.hpp ================================================ // PINOCCHIO #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include // EIGEN #include #include #include ================================================ FILE: include/simple/pinocchio_template_instantiation/aba-derivatives.txx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_pinocchio_template_instantiation_aba_derivatives_txx__ #define __simple_pinocchio_template_instantiation_aba_derivatives_txx__ extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void ::pinocchio::computeABADerivatives< ::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>, Eigen::Ref<::simple::context::RowMatrixXs>, Eigen::Ref<::simple::context::RowMatrixXs>, Eigen::Ref<::simple::context::RowMatrixXs>>( const ::simple::context::Model &, ::simple::context::Data &, const Eigen::MatrixBase<::simple::context::VectorXs> &, const Eigen::MatrixBase<::simple::context::VectorXs> &, const Eigen::MatrixBase<::simple::context::VectorXs> &, const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &, const Eigen::MatrixBase> &, const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); #endif // __simple_pinocchio_template_instantiation_aba_derivatives_txx__ ================================================ FILE: include/simple/pinocchio_template_instantiation/aba.txx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_pinocchio_template_instantiation_aba_txx__ #define __simple_pinocchio_template_instantiation_aba_txx__ extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba< ::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::VectorXs>( const ::simple::context::Model &, ::simple::context::Data &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Convention); extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba< ::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>>( const ::simple::context::Model &, ::simple::context::Data &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &, const Convention); #endif ================================================ FILE: include/simple/pinocchio_template_instantiation/crba.txx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_pinocchio_template_instantiation_crba_txx__ #define __simple_pinocchio_template_instantiation_crba_txx__ extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::MatrixXs & ::pinocchio:: crba<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs>( const context::Model &, context::Data &, const Eigen::MatrixBase &, const Convention convention); #endif // __simple_pinocchio_template_instantiation_crba_txx__ ================================================ FILE: include/simple/pinocchio_template_instantiation/joint-model.txx ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_pinocchio_template_instantiation_joint_model_txx__ #define __simple_pinocchio_template_instantiation_joint_model_txx__ extern template struct ::pinocchio:: JointModelTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>; extern template struct ::pinocchio:: JointDataTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>; #endif ================================================ FILE: include/simple/utils/visitors.hpp ================================================ // // Copyright (c) 2025 INRIA // #ifndef __simple_utils_visitors_hpp__ #define __simple_utils_visitors_hpp__ #include #include #include namespace simple { namespace visitors { // Declaration of a helper to accumulate operator() overloads from lambdas. template struct lambda_visitor_helper; // Specialization for initial case without any lambda template struct lambda_visitor_helper { // Default constructor lambda_visitor_helper() { } // Operator that handles boost::blank case ReturnType operator()(const boost::blank &) const { PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot call operator() on boost::blank."); return ::pinocchio::visitors::internal::NoRun::run(); } }; // Specialization for recursion template struct lambda_visitor_helper : Lambda , lambda_visitor_helper { typedef lambda_visitor_helper RecursiveHelper; // Initialize lambda and the recursion lambda_visitor_helper(Lambda lambda, Lambdas... lambdas) : Lambda(lambda) , RecursiveHelper(lambdas...) { } // operator() is overloaded by lambda and all overloa of the recursion using Lambda::operator(); using RecursiveHelper::operator(); }; // Wrapper struct around helper that inherit static_visitor // It is an additional class to avoid diamond inheritance template struct lambda_visitor : boost::static_visitor , lambda_visitor_helper { typedef lambda_visitor_helper Helper; lambda_visitor(Lambdas... lambdas) : Helper(lambdas...) { } using Helper::operator(); }; // Wrapper function to create a lambda_visitor instance. // This deduces the template arguments automatically, so you don't need to specify them. template lambda_visitor make_lambda_visitor(Lambdas... lambdas) { return lambda_visitor(lambdas...); } } // namespace visitors } // namespace simple #endif // __simple_utils_visitors_hpp__ ================================================ FILE: sandbox/cartpole.py ================================================ import pinocchio as pin import numpy as np import hppfcl import tap from pinocchio.visualize import MeshcatVisualizer import meshcat from simulation_args import SimulationArgs import simple GREY = np.array([192, 201, 229, 255]) / 255 class SimulationArgs(tap.Tap): horizon: int = 1000 dt: float = 1e-3 contact_solver: str = "ADMM" # set to PGS or ADMM maxit: int = 1000 # solver maxit tol: float = 1e-6 # absolute constraint solver tol tol_rel: float = 1e-6 # relative constraint solver tol solve_ncp: int = 1 # set to 0 to solve ccp warm_start: int = 1 # warm start the solver? mu_prox: float = 1e-4 # prox value for admm material: str = "metal" # contact friction compliance: float = 0 Kp: float = 0 # baumgarte proportional term Kd: float = 0 # baumgarte derivative term seed: int = 1234 limits: bool = False # set to activate limits floor: bool = False # add a floor cos_torque: bool = False dont_stop: bool = False args = SimulationArgs().parse_args() allowed_solvers = ["ADMM", "PGS"] if args.contact_solver not in allowed_solvers: print( f"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting" ) exit(1) np.random.seed(args.seed) pin.seed(args.seed) def create_cartpole(N, add_floor): model = pin.Model() geom_model = pin.GeometryModel() if add_floor: # add floor floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0) M = pin.SE3.Identity() floor_collision_object = pin.GeometryObject( "floor", 0, 0, M, floor_collision_shape ) color = GREY color[3] = 0.5 floor_collision_object.meshColor = color geom_floor = geom_model.addGeometryObject(floor_collision_object) parent_id = 0 cart_radius = 0.1 cart_length = 5 * cart_radius cart_mass = 1.0 joint_name = "joint_cart" geometry_placement = pin.SE3.Identity() geometry_placement.rotation = pin.Quaternion( np.array([0.0, 0.0, 1.0]), np.array([0.0, 1.0, 0.0]) ).toRotationMatrix() joint_id = model.addJoint( parent_id, pin.JointModelPY(), pin.SE3.Identity(), joint_name ) body_inertia = pin.Inertia.FromCylinder(cart_mass, cart_radius, cart_length) body_placement = geometry_placement model.appendBodyToJoint( joint_id, body_inertia, body_placement ) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry shape_cart = hppfcl.Cylinder(cart_radius, cart_length) geom_cart = pin.GeometryObject( "shape_cart", joint_id, geometry_placement, shape_cart ) geom_cart.meshColor = np.array([1.0, 0.1, 0.1, 1.0]) geom_model.addGeometryObject(geom_cart) parent_id = joint_id joint_placement = pin.SE3.Identity() body_mass = 1.0 body_radius = 0.1 for k in range(N): joint_name = "joint_" + str(k + 1) joint_id = model.addJoint( parent_id, pin.JointModelRX(), joint_placement, joint_name ) body_inertia = pin.Inertia.FromSphere(body_mass, body_radius) body_placement = joint_placement.copy() body_placement.translation[2] = 1.0 model.appendBodyToJoint(joint_id, body_inertia, body_placement) geom1_name = "ball_" + str(k + 1) shape1 = hppfcl.Sphere(body_radius) geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1) geom1_obj.meshColor = np.ones((4)) geom_ball = geom_model.addGeometryObject(geom1_obj) if add_floor: geom_model.addCollisionPair(pin.CollisionPair(geom_floor, geom_ball)) geom2_name = "bar_" + str(k + 1) shape2 = hppfcl.Cylinder(body_radius / 4.0, body_placement.translation[2]) shape2_placement = body_placement.copy() shape2_placement.translation[2] /= 2.0 geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2_placement, shape2) geom2_obj.meshColor = np.array([0.0, 0.0, 0.0, 1.0]) geom_model.addGeometryObject(geom2_obj) # update parent id to add next pendulum parent_id = joint_id joint_placement = body_placement.copy() end_frame = pin.Frame( "end_effector_frame", model.getJointId("joint_" + str(N)), 0, body_placement, pin.FrameType(3), ) model.addFrame(end_frame) geom_model.collision_pairs = [] model.qinit = np.zeros(model.nq) model.qinit[1] = 0.0 * np.pi model.qref = pin.neutral(model) return model, geom_model # ============================================================================ # SCENE CREATION # ============================================================================ # Create model model, geom_model = create_cartpole(1, args.floor) for gobj in geom_model.geometryObjects: if args.material == "ice": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE elif args.material == "plastic": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC elif args.material == "wood": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD elif args.material == "metal": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL elif args.material == "concrete": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE # Compliance gobj.physicsMaterial.compliance = args.compliance # Initial state if args.limits: for i in range(model.nq): model.lowerPositionLimit[i] = -0.8 model.upperPositionLimit[i] = 0.8 else: for i in range(model.nq): model.lowerPositionLimit[i] = np.finfo("d").min model.upperPositionLimit[i] = np.finfo("d").max q0 = pin.neutral(model) v0 = np.zeros(model.nv) tau0 = np.ones(model.nv) print("q0 = ", q0) print("v0 = ", v0) print(f"{model.lowerPositionLimit}") print(f"{model.upperPositionLimit}") # visualize the trajectory viewer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") viewer.delete() vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, geom_model) vizer.initViewer(viewer=viewer, open=False, loadModel=True) vizer.display(q0) # ============================================================================ # SIMULATION # ============================================================================ sim = simple.Simulator(model, geom_model) # PGS sim.pgs_constraint_solver_settings.absolute_precision = args.tol sim.pgs_constraint_solver_settings.relative_precision = args.tol_rel sim.pgs_constraint_solver_settings.max_iter = args.maxit # ADMM sim.admm_constraint_solver_settings.absolute_precision = args.tol sim.admm_constraint_solver_settings.relative_precision = args.tol_rel sim.admm_constraint_solver_settings.max_iter = args.maxit sim.admm_constraint_solver_settings.mu = args.mu_prox # sim.warm_start_contact_forces = args.warm_start sim.constraints_problem.is_ncp = args.solve_ncp sim.constraints_problem.Kp = args.Kp sim.constraints_problem.Kd = args.Kd dt = args.dt T = args.horizon q = q0.copy() v = v0.copy() tau = tau0.copy() zero_torque = np.zeros(model.nv) sim.step(q, v, tau0, dt) qprev = q.copy() vprev = v.copy() q = sim.qnew.copy() v = sim.qnew.copy() vizer.display(q) input("[Press enter to simulate]") for t in range(T): tau = np.zeros(model.nv) if args.cos_torque: tau[0] = 10 * np.cos(10 * float(t) / float(T)) if args.contact_solver == "ADMM": sim.step(q, v, tau, dt) if args.contact_solver == "PGS": sim.stepPGS(q, v, tau, dt) if sim.constraints_problem.constraints_problem_size() > 0 and not args.dont_stop: print(f"{t=}") print(f"{qprev=}") print(f"{vprev=}") print(f"{q=}") print(f"{v=}") print(f"{sim.constraints_problem.constraints_problem_size()=}") print(f"{sim.constraints_problem.constraints_forces()=}") input("[Press enter to continue]") qprev = q.copy() vprev = v.copy() q = sim.qnew.copy() v = sim.vnew.copy() vizer.display(q) ================================================ FILE: sandbox/cassie_mj.py ================================================ import pinocchio as pin import numpy as np from simulation_args import SimulationArgs import simple from robot_descriptions.loaders.pinocchio import load_robot_description as plr from sim_utils import ( addMaterialAndCompliance, addFloor, addSystemCollisionPairs, SimulationArgs, createVisualizer, printSimulationPerfStats, setupSimulatorFromArgs, plotContactSolver, subSample, runMujocoXML, ) import time args = SimulationArgs().parse_args() allowed_solvers = ["ADMM", "PGS"] if args.contact_solver not in allowed_solvers: print( f"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting" ) exit(1) np.random.seed(args.seed) pin.seed(args.seed) model_path = "cassie_mj_description" if not args.mujoco: # Create model print("Loading mj robot description...") robot_mj = plr("cassie_mj_description") model = robot_mj.model geom_model = robot_mj.collision_model visual_model = robot_mj.visual_model constraint_models_dict = robot_mj.constraint_models for key, value in constraint_models_dict.items(): print("") print(f"---Constraints of type {key}---") for constraint in value: print(f"{constraint.joint1_id=}") print(f"{constraint.joint1_placement.translation=}") print(f"{constraint.joint2_id=}") print(f"{constraint.joint2_placement.translation=}") print("") addMaterialAndCompliance(geom_model, args.material, args.compliance) print(f"{model.damping=}") # Initial state # q0 = model.referenceConfigurations["qpos0"] q0 = model.referenceConfigurations["home"] if args.random_init_vel: v0 = np.random.randn(model.nv) else: v0 = np.zeros(model.nv) # for i in range(model.nq): # model.lowerPositionLimit[i] = np.finfo("d").min # model.upperPositionLimit[i] = np.finfo("d").max # model.lowerDryFrictionLimit[:] = -0 # model.upperDryFrictionLimit[:] = 0 model.lowerDryFrictionLimit[:] = -1.0 model.upperDryFrictionLimit[:] = 1.0 model.lowerDryFrictionLimit[:6] = -0 model.upperDryFrictionLimit[:6] = 0 # for i in range(model.nq): # model.lowerPositionLimit[i] = -1.0 # model.upperPositionLimit[i] = 1.0 # print(f"{model.lowerPositionLimit=}") # print(f"{model.upperPositionLimit=}") # add floor and collision pairs addFloor(geom_model, visual_model) addSystemCollisionPairs(model, geom_model, q0) # visualize the trajectory vizer, _ = createVisualizer(model, geom_model, visual_model) vizer.display(q0) print(f"{q0=}") print(f"{v0=}") # simulation _data = model.createData() rmass = pin.computeTotalMass(model, _data) print(f"Robot mass = {rmass}") sim = simple.Simulator(model, geom_model, **constraint_models_dict) # sim = simple.Simulator(model, geom_model) setupSimulatorFromArgs(sim, args) dt = args.dt T = args.horizon input("[Press ENTER to simulate]") q = q0.copy() qs = [q0.copy()] v = v0.copy() zero_torque = np.zeros(model.nv) step_timings = np.zeros(T) for t in range(T): start_time = time.time() if args.contact_solver == "ADMM": sim.step(q, v, zero_torque, dt) if args.contact_solver == "PGS": sim.stepPGS(q, v, zero_torque, dt) end_time = time.time() step_timings[t] = end_time - start_time plotContactSolver(sim, args, t, q, v) q = sim.qnew.copy() v = sim.vnew.copy() qs.append(q) if args.display or args.debug: vizer.display(q) printSimulationPerfStats(step_timings) fps = 60.0 dt_vis = 1.0 / fps qs = subSample(qs, dt * T, fps) while True: for q in qs: step_start = time.time() vizer.display(q) time_until_next_step = dt_vis - (time.time() - step_start) if time_until_next_step > 0: time.sleep(time_until_next_step) else: runMujocoXML(model_path, args) ================================================ FILE: sandbox/force_action_derivative.py ================================================ import pinocchio as pin import numpy as np np.set_printoptions(suppress=True) # Testing the derivatives of M.act(f), where M is an SE3 element, f is a spatial force. # Derivative w.r.t M. for i in range(1000): M: pin.SE3 = pin.SE3.Random() lam: pin.Force = pin.Force.Random() # res: pin.Force = M.act(lam) # Analytic jacobian P = np.zeros((6, 6)) P[:3, 3:] = pin.skew(lam.linear) P[3:, :3] = pin.skew(lam.linear) P[3:, 3:] = pin.skew(lam.angular) J = -M.toDualActionMatrix() @ P # Finite diff jacobian Jfd = np.zeros((6, 6)) eps = 1e-6 ei = np.zeros(6) for i in range(6): ei[i] = eps M_plus = M * pin.exp6(ei) M_minus = M * pin.exp6(-ei) Jfd[:, i] = (M_plus.act(lam) - M_minus.act(lam)) / (2 * eps) ei[i] = 0 assert np.linalg.norm(Jfd - J) <= 1e-8 v = pin.Motion.Random() A = np.zeros((6, 6)) A[:3, :3] = pin.skew(v.angular) A[:3, 3:] = pin.skew(v.linear) A[3:, 3:] = pin.skew(v.angular) print(f"Action = \n{v.action}") print(f"Action alamano = \n{A}") ================================================ FILE: sandbox/four_bar_linkage.py ================================================ import pinocchio as pin import numpy as np import simple import time from sim_utils import ( addSystemCollisionPairs, SimulationArgs, createVisualizer, setupSimulatorFromArgs, plotContactSolver, subSample, runMujocoXML, printSimulationPerfStats, ) args = SimulationArgs().parse_args() model_path = "./sandbox/robots/four_bar_linkage.xml" if not args.mujoco: # create model model = pin.buildModelFromMJCF(model_path) bcms = pin.buildBilateralConstraintModelsFromMJCF(model, model_path) geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION) visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL) # initial state q0 = model.referenceConfigurations["qpos0"] v0 = np.zeros(model.nv) print(f"{q0=}") print(f"{v0=}") addSystemCollisionPairs(model, geom_model, q0) # geom_model.removeAllCollisionPairs() for inertia in model.inertias: print(inertia) num = 0.4 for i in range(model.nq): model.lowerPositionLimit[i] = -num model.upperPositionLimit[i] = num # for i in range(model.nq): # model.lowerPositionLimit[i] = np.finfo("d").min # model.upperPositionLimit[i] = np.finfo("d").max # model.lowerDryFrictionLimit[:] = -10000 # model.upperDryFrictionLimit[:] = 10000 model.lowerDryFrictionLimit[:] = 0 model.upperDryFrictionLimit[:] = 0 # visualize the trajectory vizer, _ = createVisualizer(model, geom_model, visual_model) vizer.display(q0) for inertia in model.inertias: print(inertia) print(list(bcms)) print(bcms[0]) sim = simple.Simulator(model, geom_model, bilateral_point_constraint_models=bcms) # sim = simple.Simulator(model, geom_model) setupSimulatorFromArgs(sim, args) dt = args.dt T = args.horizon input("[Press ENTER to simulate]") q = q0.copy() qs = [q0.copy()] v = v0.copy() zero_torque = np.zeros(model.nv) step_timings = np.zeros(T) for t in range(T): start_time = time.time() if args.contact_solver == "ADMM": sim.step(q, v, zero_torque, dt) if args.contact_solver == "PGS": sim.stepPGS(q, v, zero_torque, dt) end_time = time.time() step_timings[t] = end_time - start_time plotContactSolver(sim, args, t, q, v) q = sim.qnew.copy() v = sim.vnew.copy() qs.append(q) if args.display or args.debug: vizer.display(q) printSimulationPerfStats(step_timings) fps = 60.0 dt_vis = 1.0 / fps qs = subSample(qs, dt * T, fps) while True: for q in qs: step_start = time.time() vizer.display(q) time_until_next_step = dt_vis - (time.time() - step_start) if time_until_next_step > 0: time.sleep(time_until_next_step) else: runMujocoXML(model_path, args) ================================================ FILE: sandbox/four_five_bar_linkage.py ================================================ import pinocchio as pin import numpy as np import simple import time from sim_utils import ( addSystemCollisionPairs, SimulationArgs, createVisualizer, setupSimulatorFromArgs, plotContactSolver, subSample, runMujocoXML, printSimulationPerfStats, ) args = SimulationArgs().parse_args() model_path = "./sandbox/robots/four_five_bar_linkage.xml" if not args.mujoco: # create model model = pin.buildModelFromMJCF(model_path) wcms = pin.buildWeldConstraintModelsFromMJCF(model, model_path) geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION) visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL) # initial state q0 = model.referenceConfigurations["qpos0"] v0 = np.zeros(model.nv) print(f"{q0=}") print(f"{v0=}") addSystemCollisionPairs(model, geom_model, q0) # geom_model.removeAllCollisionPairs() for inertia in model.inertias: print(inertia) num = 0.4 for i in range(model.nq): model.lowerPositionLimit[i] = -num model.upperPositionLimit[i] = num # for i in range(model.nq): # model.lowerPositionLimit[i] = np.finfo("d").min # model.upperPositionLimit[i] = np.finfo("d").max # model.lowerDryFrictionLimit[:] = -10000 # model.upperDryFrictionLimit[:] = 10000 model.lowerDryFrictionLimit[:] = 0 model.upperDryFrictionLimit[:] = 0 # visualize the trajectory vizer, _ = createVisualizer(model, geom_model, visual_model) vizer.display(q0) for inertia in model.inertias: print(inertia) print(list(wcms)) print(wcms[0]) sim = simple.Simulator(model, geom_model, weld_constraint_models=[wcms[2]]) # sim = simple.Simulator(model, geom_model) setupSimulatorFromArgs(sim, args) dt = args.dt T = args.horizon input("[Press ENTER to simulate]") q = q0.copy() qs = [q0.copy()] v = v0.copy() zero_torque = np.zeros(model.nv) step_timings = np.zeros(T) for t in range(T): start_time = time.time() if args.contact_solver == "ADMM": sim.step(q, v, zero_torque, dt) if args.contact_solver == "PGS": sim.stepPGS(q, v, zero_torque, dt) end_time = time.time() step_timings[t] = end_time - start_time plotContactSolver(sim, args, t, q, v) q = sim.qnew.copy() v = sim.vnew.copy() qs.append(q) if args.display or args.debug: vizer.display(q) printSimulationPerfStats(step_timings) fps = 60.0 dt_vis = 1.0 / fps qs = subSample(qs, dt * T, fps) while True: for q in qs: step_start = time.time() vizer.display(q) time_until_next_step = dt_vis - (time.time() - step_start) if time_until_next_step > 0: time.sleep(time_until_next_step) else: runMujocoXML(model_path, args) ================================================ FILE: sandbox/go2_contact_id.py ================================================ import pinocchio as pin import numpy as np from pin_utils import addSystemCollisionPairs from simulation_utils import ( addFloor, setPhysicsProperties, ) from simulation_args import SimulationArgs import simple from compute_derivatives import computeStepDerivatives, finiteDifferencesStep import example_robot_data as erd class ScriptArgs(SimulationArgs): go1: bool = False hand_stand: bool = False display_target_traj: bool = False noptim: int = 100 step_size: float = 1e-3 linesearch: bool = False debug: bool = False cpp: bool = False linesearch: bool = False save: bool = False maxit_linesearch: int = 1000 min_step_size: float = 1e-14 finite_differences: bool = False eps_fd: float = 1e-6 args = ScriptArgs().parse_args() allowed_solvers = ["ADMM", "PGS"] if args.contact_solver not in allowed_solvers: print( f"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting" ) exit(1) np.random.seed(args.seed) pin.seed(args.seed) # ============================================================================ # SCENE CREATION # ============================================================================ # Create model if args.go1: robot = erd.load("go1") model = robot.model geom_model = robot.collision_model visual_model = robot.visual_model else: rmodel, rgeom_model, _ = pin.buildModelsFromMJCF("./robots/go2/mjcf/go2.xml") ff_model = pin.Model() ff_id = ff_model.addJoint( 0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "robot_freeflyer" ) ff_model.addJointFrame(ff_id) ff_geom_model = pin.GeometryModel() frame_id = ff_model.getFrameId("robot_freeflyer") model, geom_model = pin.appendModel( ff_model, rmodel, ff_geom_model, rgeom_model, frame_id, pin.SE3.Identity() ) # Add plane in geom_model visual_model = geom_model.copy() addFloor(geom_model, visual_model) setPhysicsProperties(geom_model, args.material, args.compliance) # Initial state # model.lowerPositionLimit = -np.ones((model.nq, 1)) # model.upperPositionLimit = np.ones((model.nq, 1)) # q0 = pin.randomConfiguration(model) if args.go1: q0 = model.referenceConfigurations["standing"] if args.hand_stand: q0 = np.array( [ 0.26, 0.0, 0.43, 0.0, 0.70710678, 0.0, 0.70710678, 0.0, -0.1, -1.6, 0.0, -0.1, -1.6, 0.0, 0.8, -1.853, 0.0, 0.8, -1.853, ] ) else: q0 = pin.neutral(model) q0[2] = 0.4 v0 = np.zeros(model.nv) fext = [pin.Force(np.random.random(6)) for _ in range(model.njoints)] print("q0 = ", q0) print("v0 = ", v0) addSystemCollisionPairs(model, geom_model, q0) actuation = np.zeros((model.nv, model.nv - 6)) actuation[6:, :] = np.eye(model.nv - 6) data = model.createData() geom_data = geom_model.createData() simulator = simple.Simulator(model, data, geom_model, geom_data) simulator.admm_constraint_solver_settings.absolute_precision = args.tol simulator.admm_constraint_solver_settings.relative_precision = args.tol_rel simulator.admm_constraint_solver_settings.max_iter = args.maxit dsim = simple.SimulatorDerivatives(simulator) def computeCost(tau): simulator.reset() simulator.step(q0, v0, actuation @ tau, args.dt) acc = (simulator.vnew - v0) / args.dt cost = 0.5 * (np.linalg.norm(acc) ** 2) return cost tau_optim_init = np.zeros(actuation.shape[1]) if args.save: costs = [] grads = [] tau_optim = tau_optim_init.copy() for n in range(args.noptim): simulator.reset() simulator.step(q0, v0, actuation @ tau_optim, args.dt) if args.finite_differences: dvnew_dq, dvnew_dv, dvnew_dtau = finiteDifferencesStep( simulator, q0, v0, actuation @ tau_optim, args.dt ) dvnew_dtau = dsim.dvnew_dtau.copy() @ actuation if args.cpp: dsim.stepDerivatives(simulator, q0, v0, actuation @ tau_optim, args.dt) dvnew_dtau = dsim.dvnew_dtau.copy() @ actuation else: dqnew_dq, dqnew_dv, dqnewdtau, dvnew_dq, dvnew_dv, dvnew_dtau = ( computeStepDerivatives( simulator, q0, v0, actuation @ tau_optim, fext, args.dt ) ) dvnew_dtau = dvnew_dtau @ actuation if args.debug: print(f"{dvnew_dtau=}") print(f"norm dvnew_dtau {np.linalg.norm(dvnew_dtau)}") q = simulator.qnew.copy() v = simulator.vnew.copy() if args.debug: input() # Compute cost acc = (simulator.vnew - v0) / args.dt cost = 0.5 * np.dot(acc, acc) # Compute cost gradient grad_cost = dvnew_dtau.T @ acc if args.save: costs.append(cost) grads.append(np.linalg.norm(grad_cost)) if args.linesearch: # Gauss newton step H_GN = np.dot(dvnew_dtau.T, dvnew_dtau) H_GN_inv = np.linalg.inv(H_GN + np.eye(model.nv - 6) * 1e-6) dtau = -H_GN_inv @ grad_cost expected_improvement = -0.5 * grad_cost @ dtau step_size = 1.0 linesearch_it = 0 while ( step_size >= args.min_step_size and linesearch_it <= args.maxit_linesearch ): tau_optim_next = tau_optim + step_size * dtau cost_next = computeCost(tau_optim_next) if cost_next < cost - step_size * expected_improvement: break step_size /= 2 linesearch_it += 1 tau_optim = tau_optim_next else: # Gradient step tau_optim -= args.step_size * grad_cost print(f"\n---- ITERATION {n} ----") print(f"Current cost = {cost}") print(f"Current norm grad cost = {np.linalg.norm(grad_cost)}") if args.save: costs = np.array(costs) grads = np.array(grads) if args.linesearch: method = "GN" else: method = "GD" if args.finite_differences: np.save(f"./results/fd_{method}_costs_invdyn.npy", costs) np.save(f"./results/fd_{method}_grads_invdyn.npy", grads) else: np.save(f"./results/{method}_costs_invdyn.npy", costs) np.save(f"./results/{method}_grads_invdyn.npy", grads) ================================================ FILE: sandbox/humanoid_mj.py ================================================ import pinocchio as pin import numpy as np import simple from sim_utils import ( # addFloor, addMaterialAndCompliance, addSystemCollisionPairs, plotContactSolver, runMujocoXML, setupSimulatorFromArgs, subSample, createVisualizer, SimulationArgs, printSimulationPerfStats, ) import time args = SimulationArgs().parse_args() allowed_solvers = ["ADMM", "PGS"] if args.contact_solver not in allowed_solvers: print( f"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting" ) exit(1) np.random.seed(args.seed) pin.seed(args.seed) model_path = "./sandbox/robots/humanoid.xml" if not args.mujoco: # Create model print("Loading mj robot description...") model = pin.buildModelFromMJCF(model_path) geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION) visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL) addMaterialAndCompliance(geom_model, args.material, args.compliance) # Initial state q0 = model.referenceConfigurations["qpos0"] for joint in model.joints: print(joint) if args.random_init_vel: v0 = np.random.randn(model.nv) else: v0 = np.zeros(model.nv) for i in range(model.nq): model.lowerPositionLimit[i] = np.finfo("d").min model.upperPositionLimit[i] = np.finfo("d").max # for i in range(model.nq): # model.positionLimitMargin[i] = np.finfo("d").max model.lowerDryFrictionLimit[:] = 0 model.upperDryFrictionLimit[:] = 0 # add floor and collision pairs # addFloor(geom_model, visual_model) addSystemCollisionPairs(model, geom_model, q0) # visualize the trajectory vizer, _ = createVisualizer(model, geom_model, visual_model) vizer.display(q0) print(f"{q0=}") print(f"{v0=}") # simulation _data = model.createData() rmass = pin.computeTotalMass(model, _data) print(f"Robot mass = {rmass}") sim = simple.Simulator(model, geom_model) setupSimulatorFromArgs(sim, args) dt = args.dt T = args.horizon N = 1 input("[Press ENTER to simulate]") for i in range(N): q = q0.copy() qs = [q0.copy()] v = v0.copy() zero_torque = np.zeros(model.nv) step_timings = np.zeros(T) sim.reset() for t in range(T): start_time = time.time() if args.contact_solver == "ADMM": sim.step(q, v, zero_torque, dt) if args.contact_solver == "PGS": sim.stepPGS(q, v, zero_torque, dt) end_time = time.time() step_timings[t] = end_time - start_time plotContactSolver(sim, args, t, q, v) q = sim.qnew.copy() v = sim.vnew.copy() qs.append(q) if args.display and args.debug: vizer.display(q) printSimulationPerfStats(step_timings) if args.display: fps = 60.0 dt_vis = 1.0 / fps qs = subSample(qs, dt * T, fps) while True: for q in qs: step_start = time.time() vizer.display(q) time_until_next_step = dt_vis - (time.time() - step_start) if time_until_next_step > 0: time.sleep(time_until_next_step) else: runMujocoXML(model_path, args) ================================================ FILE: sandbox/parallel_rollout.py ================================================ import pinocchio as pin import numpy as np import concurrent.futures import timeit import simple import os import time from simulation_utils import ( addFloor, setPhysicsProperties, ) from pin_utils import addSystemCollisionPairs from pinocchio.visualize import MeshcatVisualizer current_dir = os.path.dirname(os.path.abspath(__file__)) def createSimulator( model: pin.Model, geom_model: pin.GeometryModel, max_num_contacts: int = 4, tol: float = 1e-8, tol_rel: float = 1e-12, mu_prox: float = 1e-4, maxit: int = 1000, Kp: float = 0.0, Kd: float = 0.0, ): data = model.createData() geom_data = geom_model.createData() simulator = simple.Simulator(model, data, geom_model, geom_data) simulator.admm_constraint_solver_settings.absolute_precision = tol simulator.admm_constraint_solver_settings.relative_precision = tol_rel simulator.admm_constraint_solver_settings.max_iter = maxit simulator.admm_constraint_solver_settings.mu = mu_prox simulator.constraints_problem.setMaxNumberOfContactsPerCollisionPair( max_num_contacts ) simulator.constraints_problem.Kp = Kp simulator.constraints_problem.Kd = Kd return simulator model = pin.buildModelFromMJCF(f"{current_dir}/robots/go2/mjcf/go2.xml") geom_model = pin.buildGeomFromMJCF( model, f"{current_dir}/robots/go2/mjcf/go2.xml", pin.COLLISION ) material = "concrete" compliance = 0.0 visual_model = geom_model.copy() addFloor(geom_model, visual_model) setPhysicsProperties(geom_model, material, compliance) q = pin.neutral(model) q[2] = 0.5 v = np.zeros(model.nv) addSystemCollisionPairs(model, geom_model, q) batch_size = 1000 time_steps = 200 dt = 2e-3 # max_workers = 8 max_workers = os.cpu_count() # from: https://docs.python.org/3/library/concurrent.futures.html#concurrent.futures.ThreadPoolExecutor tau_batch = np.random.randn(batch_size, time_steps, model.nv) * 1.0 sim_batch = [createSimulator(model, geom_model) for _ in range(batch_size)] # vizer = MeshcatVisualizer(model, geom_model, visual_model) # vizer.initViewer(loadModel=True, open=True) # vizer.display(q) def simulate_tau_batch_sequential(tau_batch, sim_batch, q, v, dt): result_list = [] for tau_traj, sim in zip(tau_batch, sim_batch): sim.reset() q_ = q.copy() v_ = v.copy() for tau in tau_traj: sim.step(q_, v_, tau, dt) q_ = sim.qnew v_ = sim.vnew # vizer.display(q_) # time.sleep(0.1) result_list.append((q_, v_)) return result_list def simulate_tau_batch_parallel(sim_batch, q, v, tau_batch, dt): def rollout_single(sim, tau): q_ = q.copy() v_ = v.copy() sim.rollout(q_, v_, tau, dt) # rollout without GIL return sim.qnew, sim.vnew with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as executor: futures = [ executor.submit(rollout_single, sim, tau) for sim, tau in zip(sim_batch, tau_batch) ] results = [f.result() for f in futures] return results # check if results match results_test_sequential = simulate_tau_batch_sequential( tau_batch[:3], sim_batch[:3], q, v, dt ) results_test_parallel = simulate_tau_batch_parallel( sim_batch[:3], q, v, tau_batch[:3, :3], dt ) for i in range(3): assert np.allclose(results_test_sequential[i][0], results_test_parallel[i][0]) assert np.allclose(results_test_sequential[i][1], results_test_parallel[i][1]) print( f"Running {batch_size} rollouts with {time_steps} time steps each and dt = {dt} seconds. Overall trajectory time = {time_steps * dt} seconds." ) print(f"Number of workers: {max_workers}") execution_time_sequential = timeit.timeit( stmt="simulate_tau_batch_sequential(tau_batch, sim_batch, q, v, dt)", setup="from __main__ import simulate_tau_batch_sequential, tau_batch, sim_batch, q, v, dt", number=10, ) print(f"Execution time [sequential]: {execution_time_sequential:.6f} seconds") execution_time_parallel = timeit.timeit( stmt="simulate_tau_batch_parallel(sim_batch, q, v, tau_batch, dt)", setup="from __main__ import simulate_tau_batch_parallel, sim_batch, q, v, tau_batch, dt", number=10, ) print(f"Execution time [parallel]: {execution_time_parallel:.6f} seconds") print(f"Speedup: {execution_time_sequential / execution_time_parallel:.2f}x") ================================================ FILE: sandbox/pendulum.py ================================================ import pinocchio as pin import numpy as np import simple import time from sim_utils import ( addSystemCollisionPairs, SimulationArgs, createVisualizer, setupSimulatorFromArgs, plotContactSolver, subSample, runMujocoXML, printSimulationPerfStats, ) args = SimulationArgs().parse_args() model_path = "./sandbox/robots/pendulum.xml" if not args.mujoco: model = pin.buildModelFromMJCF(model_path) constraint_models = pin.buildConstraintModelsFromMJCF(model, model_path) geom_model = pin.buildGeomFromMJCF(model, model_path, pin.COLLISION) visual_model = pin.buildGeomFromMJCF(model, model_path, pin.VISUAL) q0 = model.referenceConfigurations["qpos0"] q0[0] = 0.4 v0 = np.zeros(model.nv) print(f"{q0=}") print(f"{v0=}") addSystemCollisionPairs(model, geom_model, q0) # geom_model.removeAllCollisionPairs() for inertia in model.inertias: print(inertia) for i in range(model.nq): model.lowerPositionLimit[i] = -0.2 model.upperPositionLimit[i] = 0.6 # for i in range(model.nq): # model.lowerPositionLimit[i] = np.finfo("d").min # model.upperPositionLimit[i] = np.finfo("d").max # model.lowerDryFrictionLimit[:] = -10000 # model.upperDryFrictionLimit[:] = 10000 # visualize the trajectory vizer, _ = createVisualizer(model, geom_model, visual_model) vizer.display(q0) # sim = simple.Simulator(model, geom_model, constraint_models) sim = simple.Simulator(model, geom_model) setupSimulatorFromArgs(sim, args) dt = args.dt T = args.horizon input("[Press ENTER to simulate]") q = q0.copy() qs = [q0.copy()] v = v0.copy() zero_torque = np.zeros(model.nv) step_timings = np.zeros(T) for t in range(T): start_time = time.time() if args.contact_solver == "ADMM": sim.step(q, v, zero_torque, dt) if args.contact_solver == "PGS": sim.stepPGS(q, v, zero_torque, dt) end_time = time.time() step_timings[t] = end_time - start_time plotContactSolver(sim, args, t, q, v) q = sim.qnew.copy() v = sim.vnew.copy() qs.append(q) if args.display or args.debug: vizer.display(q) printSimulationPerfStats(step_timings) fps = 60.0 dt_vis = 1.0 / fps qs = subSample(qs, dt * T, fps) while True: for q in qs: step_start = time.time() vizer.display(q) time_until_next_step = dt_vis - (time.time() - step_start) if time_until_next_step > 0: time.sleep(time_until_next_step) else: runMujocoXML(model_path, args) ================================================ FILE: sandbox/pin_utils.py ================================================ import hppfcl import pinocchio as pin def addSystemCollisionPairs(model, geom_model, qref): """ Add the right collision pairs of a model, given qref. qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered to always be in collision. """ data = model.createData() geom_data = geom_model.createData() pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref) geom_model.removeAllCollisionPairs() num_col_pairs = 0 for i in range(len(geom_model.geometryObjects)): for j in range(i + 1, len(geom_model.geometryObjects)): # Don't add collision pair if same object if i != j: gobj_i: pin.GeometryObject = geom_model.geometryObjects[i] gobj_j: pin.GeometryObject = geom_model.geometryObjects[j] if gobj_i.name == "floor" or gobj_j.name == "floor": num_col_pairs += 1 col_pair = pin.CollisionPair(i, j) geom_model.addCollisionPair(col_pair) else: if ( gobj_i.parentJoint != gobj_j.parentJoint or gobj_i.parentJoint == 0 ): if ( gobj_i.parentJoint != model.parents[gobj_j.parentJoint] and gobj_j.parentJoint != model.parents[gobj_i.parentJoint] or gobj_i.parentJoint == 0 or gobj_j.parentJoint == 0 ): # Compute collision between the geometries. Only add the collision pair if there is no collision. M1 = geom_data.oMg[i] M2 = geom_data.oMg[j] colreq = hppfcl.CollisionRequest() colreq.security_margin = 1e-2 # 1cm of clearance colres = hppfcl.CollisionResult() hppfcl.collide( gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres ) if not colres.isCollision(): num_col_pairs += 1 col_pair = pin.CollisionPair(i, j) geom_model.addCollisionPair(col_pair) print("Num col pairs = ", num_col_pairs) ================================================ FILE: sandbox/robots/four_bar_linkage.xml ================================================ ================================================ FILE: sandbox/robots/four_five_bar_linkage.xml ================================================ ================================================ FILE: sandbox/robots/go2/mjcf/go2.xml ================================================ ================================================ FILE: sandbox/robots/go2/mjcf/scene.xml ================================================ ================================================ FILE: sandbox/robots/humanoid.xml ================================================ ================================================ FILE: sandbox/robots/pendulum.xml ================================================ ================================================ FILE: sandbox/sim_utils.py ================================================ import pinocchio as pin import hppfcl import numpy as np import tap import simple import matplotlib.pyplot as plt import time import meshcat from pinocchio.visualize import MeshcatVisualizer try: import mujoco from robot_descriptions.loaders.mujoco import load_robot_description as mlr mujoco_imported = True except: mujoco_imported = False class SimulationArgs(tap.Tap): horizon: int = 1000 dt: float = 1e-3 contact_solver: str = "ADMM" # set to PGS or ADMM maxit: int = 100 # solver maxit tol: float = 1e-6 # absolute constraint solver tol tol_rel: float = 1e-6 # relative constraint solver tol warm_start: int = 1 # warm start the solver? mu_prox: float = 1e-4 # prox value for admm material: str = "metal" # contact friction compliance: float = 0 max_contacts_per_pair: int = 4 mujoco: bool = False Kp: float = 0 # baumgarte proportional term Kd: float = 0 # baumgarte derivative term seed: int = 1234 random_init_vel: bool = False display: bool = False display_traj: bool = False solve_ccp: bool = False debug: bool = False debug_step: int = -1 dont_stop: bool = False admm_update_rule: str = "spectral" ratio_primal_dual: float = 10 tau: float = 0.5 rho: float = 10.0 rho_power: float = 0.05 rho_power_factor: float = 0.05 lanczos_size: int = 3 linear_update_rule_factor: float = 2 def setupSimulatorFromArgs(sim: simple.Simulator, args: SimulationArgs): sim.warm_start_contact_forces = args.warm_start sim.constraints_problem.setMaxNumberOfContactsPerCollisionPair( args.max_contacts_per_pair ) sim.constraints_problem.Kp = args.Kp sim.constraints_problem.Kd = args.Kd sim.constraints_problem.is_ncp = not args.solve_ccp # PGS sim.pgs_constraint_solver_settings.stat_record = args.debug sim.pgs_constraint_solver_settings.max_iter = args.maxit sim.pgs_constraint_solver_settings.absolute_precision = args.tol sim.pgs_constraint_solver_settings.relative_precision = args.tol_rel # ADMM sim.admm_constraint_solver_settings.stat_record = args.debug sim.admm_constraint_solver_settings.max_iter = args.maxit sim.admm_constraint_solver_settings.absolute_precision = args.tol sim.admm_constraint_solver_settings.relative_precision = args.tol_rel sim.admm_constraint_solver_settings.ratio_primal_dual = args.ratio_primal_dual sim.admm_constraint_solver_settings.mu = args.mu_prox sim.admm_constraint_solver_settings.rho = args.rho sim.admm_constraint_solver_settings.tau = args.tau if args.admm_update_rule == "spectral": sim.admm_constraint_solver_settings.admm_update_rule = ( pin.ADMMUpdateRule.SPECTRAL ) sim.admm_constraint_solver_settings.rho_power_factor = args.rho_power_factor sim.admm_constraint_solver_settings.rho_power = args.rho_power sim.admm_constraint_solver_settings.lanczos_size = args.lanczos_size elif args.admm_update_rule == "linear": sim.admm_constraint_solver_settings.admm_update_rule = pin.ADMMUpdateRule.LINEAR sim.admm_constraint_solver_settings.linear_update_rule_factor = ( args.linear_update_rule_factor ) elif args.admm_update_rule == "constant": sim.admm_constraint_solver_settings.admm_update_rule = ( pin.ADMMUpdateRule.CONSTANT ) def plotContactSolver( sim: simple.Simulator, args: SimulationArgs, t: int, q: np.ndarray, v: np.ndarray, ): if args.debug or t == args.debug_step: stats: pin.SolverStats = sim.admm_constraint_solver.getStats() if args.contact_solver == "ADMM": solver = sim.admm_constraint_solver if args.contact_solver == "PGS": solver = sim.pgs_constraint_solver stats = solver.getStats() abs_res = solver.getAbsoluteConvergenceResidual() rel_res = solver.getRelativeConvergenceResidual() it = solver.getIterationCount() if stats.size() > 0: plt.cla() title = ( f"Step {t}, it = {it}, abs res = {abs_res:.2e}, rel res = {rel_res:.2e}" ) if args.contact_solver == "ADMM": title += f", cholesky count: {stats.cholesky_update_count}" plt.title(title) plt.plot(stats.primal_feasibility, label="primal feas") plt.plot(stats.dual_feasibility, label="dual feas") plt.plot(stats.dual_feasibility_ncp, label="dual feas NCP") if args.contact_solver == "ADMM": plt.plot(stats.dual_feasibility_admm, label="dual feas ADMM") plt.plot( stats.dual_feasibility_constraint, label="dual feas constraint" ) plt.plot(stats.rho, label="rho") if args.contact_solver == "PGS": plt.plot(stats.complementarity, label="complementarity") plt.yscale("log") plt.legend() plt.ion() plt.show() print(f"{t=}") print(f"{q=}") print(f"{v=}") print(f"{sim.qnew=}") print(f"{sim.vnew=}") print(f"{sim.admm_constraint_solver.getIterationCount()=}") print(f"{sim.pgs_constraint_solver.getIterationCount()=}") print(f"{sim.constraints_problem.constraints_forces=}") print(f"{sim.constraints_problem.constraints_problem_size=}") print(f"{sim.constraints_problem.joint_friction_constraint_size=}") print(f"{sim.constraints_problem.joint_limit_constraint_size=}") print(f"{sim.constraints_problem.bilateral_constraints_size=}") print(f"{sim.constraints_problem.weld_constraints_size=}") print(f"{sim.constraints_problem.frictional_point_constraints_size=}") print("Constraint solver timings: ", sim.getConstraintSolverCPUTimes().user) input("[Press enter to continue]") def subSample(xs, duration, fps): nb_frames = len(xs) nb_subframes = int(duration * fps) if nb_frames < nb_subframes: return xs else: step = nb_frames // nb_subframes xs_sub = [xs[i] for i in range(0, nb_frames, step)] return xs_sub def addSystemCollisionPairs(model, geom_model, qref): """ Add the right collision pairs of a model, given qref. qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered to always be in collision. """ data = model.createData() geom_data = geom_model.createData() pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref) geom_model.removeAllCollisionPairs() num_col_pairs = 0 for i in range(len(geom_model.geometryObjects)): for j in range(i + 1, len(geom_model.geometryObjects)): # Don't add collision pair if same object if i != j: gobj_i: pin.GeometryObject = geom_model.geometryObjects[i] gobj_j: pin.GeometryObject = geom_model.geometryObjects[j] if gobj_i.name == "floor" or gobj_j.name == "floor": num_col_pairs += 1 col_pair = pin.CollisionPair(i, j) geom_model.addCollisionPair(col_pair) else: if gobj_i.parentJoint != gobj_j.parentJoint: # Compute collision between the geometries. Only add the collision pair if there is no collision. M1 = geom_data.oMg[i] M2 = geom_data.oMg[j] colreq = hppfcl.CollisionRequest() colreq.security_margin = 1e-2 # 1cm of clearance colres = hppfcl.CollisionResult() hppfcl.collide( gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres ) if not colres.isCollision(): num_col_pairs += 1 col_pair = pin.CollisionPair(i, j) geom_model.addCollisionPair(col_pair) def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel): floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0) M = pin.SE3.Identity() floor_collision_object = pin.GeometryObject("floor", 0, 0, M, floor_collision_shape) floor_collision_object.meshColor = np.array([0.5, 0.5, 0.5, 0.5]) geom_model.addGeometryObject(floor_collision_object) h = 0.01 floor_visual_shape = hppfcl.Box(20, 20, h) Mvis = pin.SE3.Identity() Mvis.translation = np.array([0.0, 0.0, -h / 2]) floor_visual_object = pin.GeometryObject("floor", 0, 0, Mvis, floor_visual_shape) floor_visual_object.meshColor = np.array([0.5, 0.5, 0.5, 0.4]) visual_model.addGeometryObject(floor_visual_object) def addMaterialAndCompliance(geom_model, material: str, compliance: float): for gobj in geom_model.geometryObjects: if material == "ice": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE elif material == "plastic": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC elif material == "wood": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD elif material == "metal": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL elif material == "concrete": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE else: raise Exception("Material unknown.") # Compliance gobj.physicsMaterial.compliance = compliance def printSimulationPerfStats(step_timings: np.ndarray): print("============================================") print("SIMULATION") print("Time elapsed: ", np.sum(step_timings)) print( "Mean timings time step: {mean:.2f} +/- {std:.2f} microseconds".format( mean=np.mean(step_timings) * 1e6, std=np.std(step_timings) * 1e6 ) ) print( "Steps frequency: {freq:.2f} kHz".format( freq=(step_timings.size) / np.sum(step_timings) * 1e-3 ) ) print("============================================") def runMujocoXML(model_path: str, args: SimulationArgs): if not mujoco_imported: print("Can't run, need to install mujoco") if model_path.endswith(".xml"): m = mujoco.MjModel.from_xml_path(model_path) else: m = mlr(model_path) m.opt.cone = 1 # Elliptic m.opt.solver = 2 # Newton m.opt.timestep = args.dt m.opt.iterations = args.maxit m.opt.tolerance = args.tol m.opt.ls_iterations = 50 m.opt.ls_tolerance = 1e-2 d = mujoco.MjData(m) d.qpos = m.qpos0 q0 = d.qpos.copy() v0 = d.qvel.copy() a0 = d.qacc.copy() print(f"{q0=}") print(f"{v0=}") step_timings = np.zeros(args.horizon) for t in range(args.horizon): start_time = time.time() mujoco.mj_step(m, d) end_time = time.time() step_timings[t] = end_time - start_time printSimulationPerfStats(step_timings) d.qpos = q0 d.qvel = v0 d.qacc = a0 show_ui = False display_contacts = False with mujoco.viewer.launch_passive( m, d, show_left_ui=show_ui, show_right_ui=show_ui ) as viewer: input("[Press enter to display trajectory]") while True: d.qpos = q0 d.qvel = v0 d.qacc = a0 for t in range(args.horizon): step_start = time.time() # mj_step can be replaced with code that also evaluates # a policy and applies a control signal before stepping the physics. mujoco.mj_step(m, d) # Example modification of a viewer option: toggle contact points every two seconds. if display_contacts: with viewer.lock(): viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONSTRAINT] = ( 1 # int(d.time % 2) ) viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = ( 1 # int(d.time % 2) ) viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = ( 1 # int(d.time % 2) ) # Pick up changes to the physics state, apply perturbations, update options from GUI. viewer.sync() # Rudimentary time keeping, will drift relative to wall clock. time_until_next_step = m.opt.timestep - (time.time() - step_start) if args.debug: print(d.qpos) input(f"==== TIMESTEP {t} ====") if time_until_next_step > 0: time.sleep(time_until_next_step) def createVisualizer( model: pin.GeometryModel, geom_model: pin.GeometryModel, visual_model: pin.GeometryModel, ): viewer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") viewer.delete() for obj in visual_model.geometryObjects: if obj.name != "floor": color = np.random.rand(4) color[3] = 1.0 obj.meshColor = color vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, visual_model) vizer.initViewer(viewer=viewer, open=False, loadModel=True) return vizer, viewer ================================================ FILE: sandbox/simulation_args.py ================================================ import tap class SimulationArgs(tap.Tap): num_repetitions: int = 1 display: bool = False display_com: bool = False debug: bool = False debug_step: int = 1 display_collision_model: bool = False display_step: bool = False display_state: bool = False display_contacts: bool = False debug_transparency: float = 0.5 max_fps: int = 120 Kp: float = 0 # baumgarte proportional term Kd: float = 0 # baumgarte derivative term compliance: float = 0 material: str = "metal" horizon: int = 1000 dt: float = 1e-3 tol: float = 1e-6 tol_rel: float = 1e-6 mu_prox: float = 1e-4 maxit: int = 1000 warm_start: int = 1 contact_solver: str = "ADMM" plot_metrics: bool = False plot_hist: bool = False plot_title: str = "NO TITLE" seed: int = 1234 random_initial_velocity: bool = False add_damping: bool = False damping_factor: float = 0.0 admm_update_rule: str = "spectral" mujoco_show_ui: bool = False max_patch_size: int = 4 patch_tolerance: float = 1e-3 def process_args(self): if self.debug: self.display = True self.display_contacts = True self.display_state = True self.display_com = True class ControlArgs(SimulationArgs): noise_scale: float = 5.0 nnodes: int = 10 # nnodes = horizon // nsteps tau_max: float = 20.0 wtau: float = 1e-4 wtarget: float = 5.0 wvel: float = 1.0 Nsim: int = 25 max_fevals: int = 1e4 use_max_fevals: int = 1 ================================================ FILE: sandbox/simulation_utils.py ================================================ import time from typing import Dict import numpy as np import tap from viz_utils import RED, GREEN, BLUE, BLACK, PINK, GREY, BEIGE, PURPLE from viz_utils import ( register_line, register_arrowed_line, register_object, transform_object, sub_sample, ) import hppfcl import pinocchio as pin import simple import matplotlib.pyplot as plt from simulation_args import SimulationArgs class Policy: def __init__(self): pass def act( self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt ) -> np.ndarray: pass class DefaultPolicy(Policy): def __init__(self, model: pin.Model): self.actuation = None def act( self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt ) -> np.ndarray: return np.zeros(simulator.model.nv) class FreeFloatingRobotDampingPolicy(Policy): def __init__(self, model: pin.Model, damping_factor: float): self.actuation = np.zeros((model.nv, model.nv - 6)) self.actuation[6:, :] = np.eye(model.nv - 6) self.damping_factor = damping_factor def act( self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt ) -> np.ndarray: # Note: simulator and model should coincide tau_act = -self.damping_factor * v[6:] return self.actuation @ tau_act class RobotArmDampingPolicy(Policy): def __init__(self, model: pin.Model, damping_factor: float): self.actuation = np.eye(model.nv) self.damping_factor = damping_factor def act( self, simulator: simple.Simulator, q: np.ndarray, v: np.ndarray, dt ) -> np.ndarray: # Note: simulator and model should coincide tau_act = -self.damping_factor * v return self.actuation @ tau_act def setPhysicsProperties( geom_model: pin.GeometryModel, material: str, compliance: float ): for gobj in geom_model.geometryObjects: if material == "ice": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE elif material == "plastic": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC elif material == "wood": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD elif material == "metal": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL elif material == "concrete": gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE # Compliance gobj.physicsMaterial.compliance = compliance def removeBVHModelsIfAny(geom_model: pin.GeometryModel): for gobj in geom_model.geometryObjects: gobj: pin.GeometryObject bvh_types = [hppfcl.BV_OBBRSS, hppfcl.BV_OBB, hppfcl.BV_AABB] ntype = gobj.geometry.getNodeType() if ntype in bvh_types: gobj.geometry.buildConvexHull(True, "Qt") gobj.geometry = gobj.geometry.convex def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel): color = GREY color[3] = 0.5 # Collision object # floor_collision_shape = hppfcl.Box(10, 10, 2) # M = pin.SE3(np.eye(3), np.zeros(3)) # M.translation = np.array([0.0, 0.0, -(1.99 / 2.0)]) floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0) # floor_collision_shape = hppfcl.Plane(0, 0, 1, 0) # floor_collision_shape.setSweptSphereRadius(0.5) M = pin.SE3.Identity() floor_collision_object = pin.GeometryObject("floor", 0, 0, M, floor_collision_shape) floor_collision_object.meshColor = color geom_model.addGeometryObject(floor_collision_object) # Visual object floor_visual_shape = hppfcl.Box(10, 10, 0.01) floor_visual_object = pin.GeometryObject( "floor", 0, 0, pin.SE3.Identity(), floor_visual_shape ) floor_visual_object.meshColor = color visual_model.addGeometryObject(floor_visual_object) def simulateSytem( model: pin.Model, geom_model: pin.GeometryModel, visual_model: pin.GeometryModel, q0: np.ndarray, v0: np.ndarray, policy: Policy, args: Dict, ): print(f"Number of bodies in model = {model.nbodies}") print("Num geom obj = ", len(geom_model.geometryObjects)) if args.debug: for i, inertia in enumerate(model.inertias): print("------------------------") print(f"Inertia {i} =\n {inertia}") for gobj in geom_model.geometryObjects: print( f"name = {gobj.name}, {gobj.geometry.getNodeType()}, parent joint = {gobj.parentJoint}" ) if args.display: from pinocchio.visualize import MeshcatVisualizer import meshcat if args.debug: if args.display_collision_model: rendered_model = geom_model else: rendered_model = visual_model for gobj in rendered_model.geometryObjects: color = gobj.meshColor if gobj.name == "floor": color[3] = 0.2 else: color[3] = args.debug_transparency gobj.meshColor = color # visualize the trajectory viewer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") viewer.delete() viewer["/Background"].set_property("top_color", BEIGE[:3].tolist()) viewer["/Background"].set_property("bottom_color", BEIGE[:3].tolist()) viewer["/Lights/SpotLight/"].set_property("position", [-10, -10, -10]) viewer["/Lights/PointLightPositiveX/"].set_property( "position", [10, 10, 10] ) vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, visual_model) vizer.initViewer(viewer=viewer, open=False, loadModel=True) vizer.display(q0) if args.display_collision_model: vizer.displayCollisions(True) vizer.displayVisuals(False) else: vizer.displayCollisions(False) vizer.displayVisuals(True) data = model.createData() geom_data = geom_model.createData() com = pin.centerOfMass(model, data, q0) print("Center of mass = ", com) if args.display and args.debug and args.display_com: sphere_com = hppfcl.Sphere(0.05) Mcom = pin.SE3.Identity() Mcom.translation = com register_object(vizer, sphere_com, "com", Mcom, PURPLE) print(f"System total mass = {pin.computeTotalMass(model, data)}") print(f"Armature: {model.armature}") for col_req in geom_data.collisionRequests: col_req: hppfcl.CollisionRequest col_req.security_margin = 0.0 col_req.break_distance = 0.0 col_req.gjk_tolerance = 1e-6 col_req.epa_tolerance = 1e-6 col_req.gjk_initial_guess = hppfcl.GJKInitialGuess.CachedGuess col_req.gjk_variant = hppfcl.GJKVariant.DefaultGJK for patch_req in geom_data.contactPatchRequests: patch_req.setPatchTolerance(args.patch_tolerance) # Simulation parameters if args.contact_solver == "ADMM" or args.contact_solver == "PGS": simulator = simple.Simulator(model, data, geom_model, geom_data) # PGS simulator.pgs_constraint_solver_settings.absolute_precision = args.tol simulator.pgs_constraint_solver_settings.relative_precision = args.tol_rel simulator.pgs_constraint_solver_settings.max_iter = args.maxit # ADMM simulator.admm_constraint_solver_settings.absolute_precision = args.tol simulator.admm_constraint_solver_settings.relative_precision = args.tol_rel simulator.admm_constraint_solver_settings.max_iter = args.maxit simulator.admm_constraint_solver_settings.mu = args.mu_prox # simulator.warm_start_constraints_forces = args.warm_start simulator.measure_timings = True # Contact patch settings simulator.constraints_problem.setMaxNumberOfContactsPerCollisionPair( args.max_patch_size ) # Baumgarte settings simulator.constraints_problem.Kp = args.Kp simulator.constraints_problem.Kd = args.Kd if args.admm_update_rule == "spectral": simulator.admm_constraint_solver_settings.admm_update_rule = ( pin.ADMMUpdateRule.SPECTRAL ) elif args.admm_update_rule == "linear": simulator.admm_constraint_solver_settings.admm_update_rule = ( pin.ADMMUpdateRule.LINEAR ) else: print(f"ERROR - no match for admm update rule {args.admm_update_rule}") exit(1) dt = args.dt T = args.horizon print( f"[Main simulation will be repeated {args.num_repetitions} times to gather timings]" ) if args.display: input("[Press enter to simulate.]") step_timings = 0 contact_solver_timings = 0 for _ in range(args.num_repetitions): q = q0.copy() v = v0.copy() tau = np.zeros(model.nv) fext = [pin.Force(np.zeros(6)) for _ in range(model.njoints)] simulator.resetBeforeLooping() for t in range(T): tau = policy.act(simulator, q, v, dt) if args.contact_solver == "ADMM": simulator.step(q, v, tau, fext, dt) else: simulator.stepPGS(q, v, tau, fext, dt) if args.debug and t % args.debug_step == 0: if args.display: vizer.display(simulator.qnew) if args.display_com: com = pin.centerOfMass(model, data, simulator.qnew) Mcom = pin.SE3.Identity() Mcom.translation = com transform_object(vizer, sphere_com, "com", Mcom) print(f"\n========== TIMESTEP {t} ===========") num_contacts = simulator.constraints_problem.getNumberOfContacts() print(f"===> Num contact points = {num_contacts}") print( f"===> Timings of contact solver = { simulator.getContactSolverCPUTimes().user } us" ) print( f"===> Timings of step function = { simulator.getStepCPUTimes().user } us" ) if args.display_state: print(f"===> Joint config [q] =\n {q}") print(f"===> Joint vel [v] =\n {v}") print(f" ----> v.norm() = {np.linalg.norm(v)}") print(f"===> Joint torque [tau] =\n {tau}") print(f"===> Free joint vel [vfree] =\n {simulator.vfree}") print(f"===> Updated joint config [qnew] =\n {simulator.qnew}") print(f"===> Updated joint vel [vnew] =\n {simulator.vnew}") print(f" ----> vnew.norm() = {np.linalg.norm(simulator.vnew)}") frictional_point_constraints_forces = simulator.constraints_problem.frictional_point_constraints_forces() print( f"===> Contact forces [frictional_point_constraints_forces()] =\n {frictional_point_constraints_forces}" ) print( f" ----> frictional_point_constraints_forces().norm() = {np.linalg.norm(frictional_point_constraints_forces)}" ) print("===> Total forces (external + contact):") for i in range(model.njoints): print( f" ---> {model.names[i]}, ftotal =\n{simulator.ftotal[i]}" ) if args.display_contacts: vizer.viewer["contact_info"].delete() print("===> Contact information:") for i in range(num_contacts): # Display contact point cp = simulator.constraints_problem.contact_points[i] sphere = hppfcl.Sphere(0.01) cp_name = f"contact_info/contact_point_{i}" M = pin.SE3.Identity() M.translation = cp register_object(vizer, sphere, cp_name, M, BLACK) # Display contact force print( f" --> cp {i} = ", cp, ) normal = simulator.constraints_problem.contact_normals[i] print( f" --> normal {i} = ", normal, ) frictional_point_constraints_forces = simulator.constraints_problem.frictional_point_constraints_forces() fcontact = frictional_point_constraints_forces[ 3 * i : 3 * i + 3 ] print( f" --> contact force {i} = {fcontact}", ) constraint_model: pin.RigidConstraintModel = ( simulator.constraints_problem.getConstraintModel(i) ) joint1_id = constraint_model.joint1_id i1Mc = constraint_model.joint1_placement wMc = data.oMi[joint1_id].act(i1Mc) spatial_force_loc = pin.Force(fcontact, np.zeros(3)) spatial_force: pin.Force = wMc.act(spatial_force_loc) # To give a nice visual of the contact force, I'll assume the following: # 1Kg of force (~10 Newtons) is 0.1 meters -> so we need to divide the contact # force by 100. visual_factor = 1e-2 # Note: the - sign is because the normal goes from body 1 to body 2, but we # want to view the force exerted by body 2 on body 1. # new_cp = cp - normal * fcontact[2] * visual_factor new_cp = cp - spatial_force.linear * visual_factor force_arrow_name = f"contact_info/contact_force_{i}" register_arrowed_line( vizer, cp, new_cp, force_arrow_name, 0.005, RED ) if args.display_step: # Re-print solver info, nice for debugging print(f"===> Num contact points = {num_contacts}") print( f"===> Timings of contact solver = { simulator.getContactSolverCPUTimes().user } us" ) print( f"===> Timings of step function = { simulator.getStepCPUTimes().user } us" ) input(f"[Timestep {t} - press enter to continue.]") # Update simulator state step_timings += simulator.getStepCPUTimes().user contact_solver_timings += simulator.getContactSolverCPUTimes().user q = simulator.qnew.copy() v = simulator.vnew.copy() step_timings *= 1e-6 # convert micro seconds to seconds print("============================================") print("SIMULATION") print("Time elapsed: ", step_timings) print("Mean timings time step: ", step_timings / (T * args.num_repetitions)) print("Steps per second: ", (T * args.num_repetitions) / (step_timings)) print( "Mean timings contact solver: ", contact_solver_timings / (T * args.num_repetitions), ) print("============================================") if args.display: # remove contact info if any vizer.viewer["contact_info"].delete() # recompute and store trajectory print("[Recomputing trajectory for displaying it...]") q, v = q0.copy(), v0.copy() fext = [pin.Force(np.zeros(6)) for _ in range(model.njoints)] tau = np.zeros(model.nv) qs, vs = [q], [v] numit = [] step_timings = [] contact_solver_timings = [] vnorm = [] contact_forces_norm = [] num_contacts = [] mechanical_energy = [] potential_energy = [] kinetic_energy = [] simulator.resetBeforeLooping() for t in range(T): tau = policy.act(simulator, q, v, dt) if args.contact_solver == "ADMM": simulator.step(q, v, tau, fext, dt) else: simulator.stepPGS(q, v, tau, fext, dt) q = simulator.qnew.copy() v = simulator.vnew.copy() # # Save trajectory for display qs.append(q.copy()) vs.append(v.copy()) # # Save metrics vnorm.append(np.linalg.norm(v)) nc = simulator.constraints_problem.getNumberOfContacts() num_contacts.append(nc) contact_forces = simulator.constraints_problem.contact_forces() contact_forces_norm.append(np.linalg.norm(contact_forces)) numit.append(simulator.admm_constraint_solver.getIterationCount()) step_timings.append(simulator.getStepCPUTimes().user) contact_solver_timings.append(simulator.getContactSolverCPUTimes().user) mechanical_energy.append(pin.computeMechanicalEnergy(model, data, q, v)) potential_energy.append(pin.computePotentialEnergy(model, data, q)) kinetic_energy.append(pin.computeKineticEnergy(model, data, q, v)) if args.plot_metrics: plt.figure() plt.plot(vs, label=[f"Joint {i}" for i in range(model.nv)]) plt.xlabel("Timestep") plt.ylabel("Joint velocities") plt.legend() plt.ion() plt.show() plt.figure() plt.plot(mechanical_energy, label="Mechanical energy") plt.plot(potential_energy, label="Potential energy") plt.plot(kinetic_energy, label="Kinetic energy") plt.xlabel("Timestep") plt.ylabel("Energy") plt.legend() plt.ion() plt.show() fontsize = 12 fig, ax = plt.subplots(nrows=2, ncols=3, figsize=(16, 10)) if args.plot_hist: ax[0, 0].hist( numit, bins=args.maxit // 2, align="mid", color=PURPLE, density=True, histtype="bar", cumulative=False, linewidth=3, rwidth=0.8, ) ax[0, 0].set_xlabel("# of iterations", fontsize=fontsize) ax[0, 0].set_ylabel("Ratio of problems", fontsize=fontsize) ax[0, 0].set_title( f"{args.contact_solver} solver iterations", fontsize=fontsize, ) else: # Number of iterations along trajectory ax[0, 0].plot(numit, "+", color=PURPLE, linewidth=1) ax[0, 0].set_xlabel("Timestep", fontsize=fontsize) ax[0, 0].set_ylabel("# of iterations", fontsize=fontsize) ax[0, 0].set_title( f"{args.contact_solver} solver iterations along trajectory", fontsize=fontsize, ) if args.plot_hist: # Distribution of number of contact points num_contacts = np.array(num_contacts, dtype=np.int32) ax[0, 1].hist( num_contacts, align="mid", color=PURPLE, density=True, histtype="bar", cumulative=False, linewidth=3, rwidth=0.8, ) ax[0, 1].set_xlabel("# contact points", fontsize=fontsize) ax[0, 1].set_ylabel("Ratio of problems", fontsize=fontsize) ax[0, 1].set_title( "Distribution # of contact points", fontsize=fontsize, ) else: # Number of contacts along trajectory ax[0, 1].plot(num_contacts, "+", color=PURPLE, linewidth=1) ax[0, 1].set_xlabel("Timestep", fontsize=fontsize) ax[0, 1].set_ylabel("# of contact points", fontsize=fontsize) ax[0, 1].set_title( f"Number of contact points along trajectory", fontsize=fontsize, ) # Contact solver timings ax[0, 2].plot(contact_solver_timings, "+", linewidth=3, color=PURPLE) ax[0, 2].set_xlabel("Timestep", fontsize=fontsize) ax[0, 2].set_ylabel("Contact solver timings", fontsize=fontsize) ax[0, 2].set_title( "Contact solver timings along trajectory", fontsize=fontsize, ) # Joint velocity ax[1, 0].plot(vnorm, linewidth=3, color=PURPLE) ax[1, 0].set_xlabel("Timestep", fontsize=fontsize) ax[1, 0].set_ylabel("Joint vel norm", fontsize=fontsize) ax[1, 0].set_title( "Joint velocity norm along trajectory", fontsize=fontsize, ) # Contact forces ax[1, 1].plot(contact_forces_norm, "+", linewidth=1, color=PURPLE) ax[1, 1].set_xlabel("Timestep", fontsize=fontsize) ax[1, 1].set_ylabel("Contact forces norm", fontsize=fontsize) ax[1, 1].set_yscale("log") ax[1, 1].set_title( "Contact forces norm along trajectory", fontsize=fontsize, ) # Step timings ax[1, 2].plot(step_timings, "+", linewidth=3, color=PURPLE) ax[1, 2].set_xlabel("Timestep", fontsize=fontsize) ax[1, 2].set_ylabel("`Step` timings", fontsize=fontsize) ax[1, 2].set_title( "`Step` timings along trajectory", fontsize=fontsize, ) plt.suptitle( f"{args.plot_title}\ntol = {args.tol}, maxit = {args.maxit}, dt = {args.dt}, horizon = {args.horizon}" ) plt.ion() plt.show() max_fps = args.max_fps fps = min([max_fps, 1.0 / dt]) dt_vis = 1.0 / float(fps) qs = sub_sample(qs, dt * T, fps) vizer.display(qs[0]) input("[Press enter to display simulated trajectory]") while True: for t in range(len(qs)): step_start = time.time() vizer.display(qs[t]) time_until_next_step = dt_vis - (time.time() - step_start) if time_until_next_step > 0: time.sleep(time_until_next_step) ================================================ FILE: sandbox/test_memory.py ================================================ import pinocchio as pin import numpy as np from sim_utils import SimulationArgs, runMujocoXML import mujoco import os args = SimulationArgs().parse_args() allowed_solvers = ["ADMM", "PGS"] if args.contact_solver not in allowed_solvers: print( f"Error: unsupported simulator. Avalaible simulators: {allowed_solvers}. Exiting" ) exit(1) np.random.seed(args.seed) pin.seed(args.seed) current_dir = os.path.dirname(os.path.abspath(__file__)) # model_path = "./sandbox/robots/humanoid.xml" model_path = f"{current_dir}/robots/go2/mjcf/scene.xml" # runMujocoXML(model_path, args) n = 1000 models = [] datas = [] for _ in range(n): m = mujoco.MjModel.from_xml_path(model_path) m.opt.solver = 0 d = mujoco.MjData(m) # print(d.ncon) models.append(m) datas.append(d) for i in range(n): m = models[i] d = datas[i] mujoco.mj_step(m, d) # print(d.ncon) input("PRESS ENTER TO SWITCH TO PINOCCHIO") import pinocchio as pin import numpy as np import simple import os from simulation_utils import ( addFloor, setPhysicsProperties, ) from pin_utils import addSystemCollisionPairs current_dir = os.path.dirname(os.path.abspath(__file__)) def createSimulator( model: pin.Model, geom_model: pin.GeometryModel, max_num_contacts: int = 4, tol: float = 1e-8, tol_rel: float = 1e-12, mu_prox: float = 1e-4, maxit: int = 1000, Kp: float = 0.0, Kd: float = 0.0, ): data = model.createData() geom_data = geom_model.createData() simulator = simple.Simulator(model, data, geom_model, geom_data) simulator.admm_constraint_solver_settings.absolute_precision = tol simulator.admm_constraint_solver_settings.relative_precision = tol_rel simulator.admm_constraint_solver_settings.max_iter = maxit simulator.admm_constraint_solver_settings.mu = mu_prox simulator.constraints_problem.setMaxNumberOfContactsPerCollisionPair( max_num_contacts ) simulator.constraints_problem.Kp = Kp simulator.constraints_problem.Kd = Kd return simulator model = pin.buildModelFromMJCF(f"{current_dir}/robots/go2/mjcf/go2.xml") geom_model = pin.buildGeomFromMJCF( model, f"{current_dir}/robots/go2/mjcf/go2.xml", pin.COLLISION ) print("Done loading pinocchio model") material = "concrete" compliance = 0.0 visual_model = geom_model.copy() addFloor(geom_model, visual_model) setPhysicsProperties(geom_model, material, compliance) q = pin.neutral(model) q[2] = 0.5 v = np.zeros(model.nv) addSystemCollisionPairs(model, geom_model, q) print("Creating simple simulators...") simulators = [] for _ in range(n): sim = createSimulator(model, geom_model) simulators.append(sim) input("EOF") ================================================ FILE: sandbox/viz_utils.py ================================================ import hppfcl import numpy as np import meshcat import meshcat.geometry as mg import pinocchio as pin from pinocchio.visualize import MeshcatVisualizer import warnings from typing import Any, Dict, Union, List MsgType = Dict[str, Union[str, bytes, bool, float, "MsgType"]] RED = np.array([232, 114, 84, 255]) / 255 GREEN = np.array([84, 232, 121, 255]) / 255 BLUE = np.array([96, 86, 232, 255]) / 255 BLACK = np.array([58, 60, 69, 255]) / 255 PINK = np.array([239, 47, 201, 255]) / 255 GREY = np.array([192, 201, 229, 255]) / 255 BEIGE = np.array([252, 247, 234, 255]) / 255 PURPLE = np.array([161, 34, 183, 255]) / 255 def sub_sample(xs, duration, fps): nb_frames = len(xs) nb_subframes = int(duration * fps) if nb_frames < nb_subframes: return xs else: step = nb_frames // nb_subframes xs_sub = [xs[i] for i in range(0, nb_frames, step)] return xs_sub def rgbToHex(color): if len(color) == 4: c = color[:3] opacity = color[3] else: c = color opacity = 1.0 hex_color = "0x%02x%02x%02x" % (int(c[0] * 255), int(c[1] * 255), int(c[2] * 255)) return hex_color, opacity def register_object( viz: MeshcatVisualizer, shape: hppfcl.ShapeBase, shape_name: str, M: pin.SE3, shape_color=np.ones(4), ) -> int: meshcat_shape = load_primitive(shape) if isinstance(shape, (hppfcl.Plane, hppfcl.Halfspace)): T = M.copy() T.translation += M.rotation @ (shape.d * shape.n) T = T.homogeneous else: T = M.homogeneous # Update viewer configuration. viz.viewer[shape_name].set_object(meshcat_shape, meshcat_material(*shape_color)) viz.viewer[shape_name].set_transform(T) def register_line( viz: MeshcatVisualizer, pt1: np.ndarray, pt2: np.ndarray, line_name: str, linewidth: float = 0.01, color: np.ndarray = BLACK, ) -> int: height = np.linalg.norm(pt2 - pt1) if height > 1e-6: axis_ref = np.array([0.0, 0.0, 1.0]) axis = (pt2 - pt1) / height # - np.array([0., 0., 1.]) num = np.outer(axis_ref + axis, axis_ref + axis) den = np.dot(axis_ref + axis, axis_ref + axis) if den > 1e-6: R = 2 * num / den - np.eye(3) else: R = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) t = pt1 translation = pin.SE3(np.eye(3), np.array([0.0, 0.0, height / 2])) M = pin.SE3(R, t) Mtot = M * translation cylinder = hppfcl.Cylinder(linewidth, height) meshcat_shape = load_primitive(cylinder) viz.viewer[line_name].set_object(meshcat_shape, meshcat_material(*color)) viz.viewer[line_name].set_transform(Mtot.homogeneous) def register_arrowed_line( viz: MeshcatVisualizer, pt1: np.ndarray, pt2: np.ndarray, line_name: str, linewidth: float = 0.01, color: np.ndarray = BLACK, ) -> int: height = np.linalg.norm(pt2 - pt1) if height > 1e-6: register_line(viz, pt1, pt2, line_name, linewidth, color) arrow: hppfcl.Convex = create_arrow_head(4 * linewidth) M = pin.SE3(np.eye(3), np.array([0.0, 0.0, height / 2])) arrow_shape = load_primitive(arrow) arrow_name = line_name + "/arrow" viz.viewer[arrow_name].set_object(arrow_shape, meshcat_material(*color)) viz.viewer[arrow_name].set_transform(M.homogeneous) def transform_object( viz: MeshcatVisualizer, shape: hppfcl.ShapeBase, shape_name: str, M: pin.SE3 ) -> None: if isinstance(shape, (hppfcl.Plane, hppfcl.Halfspace)): T = M.copy() T.translation += M.rotation @ (shape.d * shape.n) T = T.homogeneous else: T = M.homogeneous # Update viewer configuration. viz.viewer[shape_name].set_transform(T) return def delete_object(viz: MeshcatVisualizer, name: str): viz.viewer[name].delete() TWOPI = 2 * np.pi def create_arrow_head(scale_: float = 1.0, n: int = 10) -> hppfcl.Convex: scale = scale_ / 2 pts = hppfcl.StdVec_Vec3f() assert n > 3 center = np.zeros(3) for i in range(n): pt = scale * np.array([np.cos(TWOPI * i / n), np.sin(TWOPI * i / n), 0.0]) center += pt pts.append(pt) pts.append(center) pts.append(scale * np.array([0.0, 0.0, 2.0])) tris = hppfcl.StdVec_Triangle() for i in range(n): # Base triangle tris.append(hppfcl.Triangle(i, n, (i + 1) % n)) # Side triangle tris.append(hppfcl.Triangle(i, (i + 1) % n, n + 1)) cvx = hppfcl.Convex(pts, tris) return cvx def npToTTuple(M): L = M.tolist() for i in range(len(L)): L[i] = tuple(L[i]) return tuple(L) def npToTuple(M): if len(M.shape) == 1: return tuple(M.tolist()) if M.shape[0] == 1: return tuple(M.tolist()[0]) if M.shape[1] == 1: return tuple(M.T.tolist()[0]) return npToTTuple(M) def load_primitive(geom: hppfcl.ShapeBase): import meshcat.geometry as mg # Cylinders need to be rotated basic_three_js_transform = np.array( [ [1.0, 0.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0], ] ) RotatedCylinder = type( "RotatedCylinder", (mg.Cylinder,), {"intrinsic_transform": lambda self: basic_three_js_transform}, ) # Cones need to be rotated if isinstance(geom, hppfcl.Capsule): if hasattr(mg, "TriangularMeshGeometry"): obj = createCapsule(2.0 * geom.halfLength, geom.radius) else: obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius) elif isinstance(geom, hppfcl.Cylinder): obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius) elif isinstance(geom, hppfcl.Cone): obj = RotatedCylinder(2.0 * geom.halfLength, 0, geom.radius, 0) elif isinstance(geom, hppfcl.Box): obj = mg.Box(npToTuple(2.0 * geom.halfSide)) elif isinstance(geom, hppfcl.Sphere): obj = mg.Sphere(geom.radius) elif isinstance(geom, hppfcl.TriangleP): faces = np.empty((1, 3), dtype=int) vertices = np.empty((3, 3)) vertices[0] = geom.a vertices[1] = geom.b vertices[2] = geom.c faces[0] = [0, 1, 2] obj = mg.TriangularMeshGeometry(vertices, faces) elif isinstance(geom, hppfcl.Ellipsoid): obj = mg.Ellipsoid(geom.radii) elif isinstance(geom, (hppfcl.Plane, hppfcl.Halfspace)): plane_transform: pin.SE3 = pin.SE3.Identity() # plane_transform.translation[:] = geom.d # Does not work plane_transform.rotation = pin.Quaternion.FromTwoVectors( geom.n, pin.ZAxis ).toRotationMatrix() TransformedPlane = type( "TransformedPlane", (Plane,), {"intrinsic_transform": lambda self: plane_transform.homogeneous}, ) obj = TransformedPlane(10, 10) elif isinstance(geom, hppfcl.ConvexBase): obj = loadMesh(geom) else: msg = "Unsupported geometry type for (%s)" % (type(geom)) warnings.warn(msg, category=UserWarning, stacklevel=2) obj = None return obj def loadMesh(mesh): if isinstance(mesh, (hppfcl.Convex, hppfcl.BVHModelBase)): if isinstance(mesh, hppfcl.BVHModelBase): num_vertices = mesh.num_vertices num_tris = mesh.num_tris call_triangles = mesh.tri_indices call_vertices = mesh.vertices elif isinstance(mesh, hppfcl.Convex): num_vertices = mesh.num_points num_tris = mesh.num_polygons call_triangles = mesh.polygons call_vertices = mesh.points faces = np.empty((num_tris, 3), dtype=int) for k in range(num_tris): tri = call_triangles(k) faces[k] = [tri[i] for i in range(3)] vertices = call_vertices() vertices = vertices.astype(np.float32) if num_tris > 0: mesh = mg.TriangularMeshGeometry(vertices, faces) else: mesh = mg.Points( mg.PointsGeometry( vertices.T, color=np.repeat(np.ones((3, 1)), num_vertices, axis=1) ), mg.PointsMaterial(size=0.002), ) return mesh def createCapsule(length, radius, radial_resolution=30, cap_resolution=10): nbv = np.array([max(radial_resolution, 4), max(cap_resolution, 4)]) h = length r = radius position = 0 vertices = np.zeros((nbv[0] * (2 * nbv[1]) + 2, 3)) for j in range(nbv[0]): phi = (2 * np.pi * j) / nbv[0] for i in range(nbv[1]): theta = (np.pi / 2 * i) / nbv[1] vertices[position + i, :] = np.array( [ np.cos(theta) * np.cos(phi) * r, np.cos(theta) * np.sin(phi) * r, -h / 2 - np.sin(theta) * r, ] ) vertices[position + i + nbv[1], :] = np.array( [ np.cos(theta) * np.cos(phi) * r, np.cos(theta) * np.sin(phi) * r, h / 2 + np.sin(theta) * r, ] ) position += nbv[1] * 2 vertices[-2, :] = np.array([0, 0, -h / 2 - r]) vertices[-1, :] = np.array([0, 0, h / 2 + r]) indexes = np.zeros((nbv[0] * (4 * (nbv[1] - 1) + 4), 3)) index = 0 stride = nbv[1] * 2 last = nbv[0] * (2 * nbv[1]) + 1 for j in range(nbv[0]): j_next = (j + 1) % nbv[0] indexes[index + 0] = np.array( [j_next * stride + nbv[1], j_next * stride, j * stride] ) indexes[index + 1] = np.array( [j * stride + nbv[1], j_next * stride + nbv[1], j * stride] ) indexes[index + 2] = np.array( [j * stride + nbv[1] - 1, j_next * stride + nbv[1] - 1, last - 1] ) indexes[index + 3] = np.array( [j_next * stride + 2 * nbv[1] - 1, j * stride + 2 * nbv[1] - 1, last] ) for i in range(nbv[1] - 1): indexes[index + 4 + i * 4 + 0] = np.array( [j_next * stride + i, j_next * stride + i + 1, j * stride + i] ) indexes[index + 4 + i * 4 + 1] = np.array( [j_next * stride + i + 1, j * stride + i + 1, j * stride + i] ) indexes[index + 4 + i * 4 + 2] = np.array( [ j_next * stride + nbv[1] + i + 1, j_next * stride + nbv[1] + i, j * stride + nbv[1] + i, ] ) indexes[index + 4 + i * 4 + 3] = np.array( [ j_next * stride + nbv[1] + i + 1, j * stride + nbv[1] + i, j * stride + nbv[1] + i + 1, ] ) index += 4 * (nbv[1] - 1) + 4 return mg.TriangularMeshGeometry(vertices, indexes) class Plane(mg.Geometry): """A plane of the given width and height.""" def __init__( self, width: float, height: float, widthSegments: float = 1, heightSegments: float = 1, ): super().__init__() self.width = width self.height = height self.widthSegments = widthSegments self.heightSegments = heightSegments def lower(self, object_data: Any) -> MsgType: return { "uuid": self.uuid, "type": "PlaneGeometry", "width": self.width, "height": self.height, "widthSegments": self.widthSegments, "heightSegments": self.heightSegments, } def meshcat_material(r, g, b, a): material = mg.MeshPhongMaterial() material.color = int(r * 255) * 256**2 + int(g * 255) * 256 + int(b * 255) material.opacity = a return material def create_visualizer( grid: bool = False, axes: bool = False, zmq_url="tcp://127.0.0.1:6000" ) -> meshcat.Visualizer: vis = meshcat.Visualizer(zmq_url=zmq_url) # vis = meshcat.Visualizer() vis.delete() if not grid: vis["/Grid"].set_property("visible", False) if not axes: vis["/Axes"].set_property("visible", False) return vis ================================================ FILE: sources.cmake ================================================ # Define Simple sources and headers # Core library set(${PROJECT_NAME}_CORE_SOURCES empty.cpp) set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/simple/utils/visitors.hpp ${PROJECT_SOURCE_DIR}/include/simple/core/fwd.hpp ${PROJECT_SOURCE_DIR}/include/simple/core/contact-frame.hpp ${PROJECT_SOURCE_DIR}/include/simple/core/contact-frame.hxx ${PROJECT_SOURCE_DIR}/include/simple/core/constraints-problem.hpp ${PROJECT_SOURCE_DIR}/include/simple/core/constraints-problem.hxx ${PROJECT_SOURCE_DIR}/include/simple/core/simulator.hpp ${PROJECT_SOURCE_DIR}/include/simple/core/simulator.hxx ${PROJECT_SOURCE_DIR}/include/simple/math/fwd.hpp ${PROJECT_SOURCE_DIR}/include/simple/math/qr.hpp ${PROJECT_SOURCE_DIR}/include/simple/fwd.hpp) set(_binary_headers_root ${${PROJECT_NAME}_BINARY_DIR}/include/simple) set(${PROJECT_NAME}_CORE_GENERATED_PUBLIC_HEADERS ${_binary_headers_root}/config.hpp ${_binary_headers_root}/deprecated.hpp ${_binary_headers_root}/warning.hpp) # Template instantiation set(${PROJECT_NAME}_TEMPLATE_INSTANTIATION_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/simple/core/constraints-problem.txx ${PROJECT_SOURCE_DIR}/include/simple/core/simulator.txx) set(${PROJECT_NAME}_TEMPLATE_INSTANTIATION_SOURCES core/constraints-problem.cpp core/simulator.cpp) set(${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_SOURCES pinocchio_template_instantiation/aba-derivatives.cpp pinocchio_template_instantiation/aba.cpp pinocchio_template_instantiation/crba.cpp pinocchio_template_instantiation/joint-model.cpp) set(${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_HEADERS ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/aba-derivatives.txx ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/aba.txx ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/crba.txx ${PROJECT_SOURCE_DIR}/include/simple/pinocchio_template_instantiation/joint-model.txx) # Python bindings set(${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/simple/bindings/python/fwd.hpp ${PROJECT_SOURCE_DIR}/include/simple/bindings/python/core/constraints-problem.hpp ${PROJECT_SOURCE_DIR}/include/simple/bindings/python/core/simulator.hpp) set(${PROJECT_NAME}_BINDINGS_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/bindings/python/core/expose-contact-frame.cpp ${PROJECT_SOURCE_DIR}/bindings/python/core/expose-constraints-problem.cpp ${PROJECT_SOURCE_DIR}/bindings/python/core/expose-simulator.cpp ${PROJECT_SOURCE_DIR}/bindings/python/module.cpp) ================================================ FILE: src/CMakeLists.txt ================================================ # # Copyright (c) 2024 INRIA # # Create header-only target All other target will depend on it. add_library(${PROJECT_NAME}_header_only INTERFACE ${${PROJECT_NAME}_HEADERS}) # On CMake 3.16, we can't target_sources(${PROJECT_NAME}_header_only INTERFACE # ${${PROJECT_NAME}_CORE_PUBLIC_HEADERS}) if(INITIALIZE_WITH_NAN) target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE EIGEN_INITIALIZE_MATRICES_BY_NAN) endif() if(CHECK_RUNTIME_MALLOC) target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE SIMPLE_EIGEN_CHECK_MALLOC EIGEN_RUNTIME_NO_MALLOC) endif(CHECK_RUNTIME_MALLOC) if(ENABLE_TEMPLATE_INSTANTIATION) target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE SIMPLE_ENABLE_TEMPLATE_INSTANTIATION) endif() target_link_libraries(${PROJECT_NAME}_header_only INTERFACE Eigen3::Eigen pinocchio::pinocchio Boost::boost Boost::serialization) # add precompiled header for pinocchio and eigen headers if(USE_PRECOMPILED_HEADERS) target_precompile_headers(${PROJECT_NAME}_header_only INTERFACE ${PROJECT_SOURCE_DIR}/include/simple/pch.hpp) endif() # Enable diffcoal for collision detection derivatives if(SIMPLE_USE_DIFFCOAL) target_link_libraries(${PROJECT_NAME}_header_only INTERFACE diffcoal::diffcoal) else(SIMPLE_USE_DIFFCOAL) target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS) endif(SIMPLE_USE_DIFFCOAL) # Enable tracy profiling if(SIMPLE_TRACY_ENABLE) modernize_target_link_libraries( ${PROJECT_NAME}_header_only SCOPE INTERFACE TARGETS Tracy::TracyClient) endif() target_include_directories(${PROJECT_NAME}_header_only INTERFACE $) cxx_flags_by_compiler_frontend( MSVC "/bigobj" OUTPUT PUBLIC_OPTIONS FILTER) target_compile_options(${PROJECT_NAME}_header_only INTERFACE ${PUBLIC_OPTIONS}) cxx_flags_by_compiler_frontend(MSVC "NOMINMAX" OUTPUT PUBLIC_DEFINITIONS) target_compile_definitions(${PROJECT_NAME}_header_only INTERFACE ${PUBLIC_DEFINITIONS}) add_header_group(${PROJECT_NAME}_CORE_PUBLIC_HEADERS) add_header_group(${PROJECT_NAME}_CORE_GENERATED_PUBLIC_HEADERS) install( TARGETS ${PROJECT_NAME}_header_only EXPORT ${TARGETS_EXPORT_NAME} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) if(ENABLE_TEMPLATE_INSTANTIATION) add_library( ${PROJECT_NAME}_pinocchio_ti SHARED ${${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_SOURCES} ${${PROJECT_NAME}_PINOCCHIO_TEMPLATE_INSTANTIATION_HEADERS}) target_link_libraries(${PROJECT_NAME}_pinocchio_ti PUBLIC ${PROJECT_NAME}_header_only) install( TARGETS ${PROJECT_NAME}_pinocchio_ti EXPORT ${TARGETS_EXPORT_NAME} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) endif() add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_CORE_SOURCES}) target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_header_only) if(ENABLE_TEMPLATE_INSTANTIATION) target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_pinocchio_ti) endif() install( TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) ================================================ FILE: src/core/constraints-problem.cpp ================================================ // // Copyright (c) 2022-2024 INRIA // #include "simple/core/constraints-problem.hpp" namespace simple { template struct ConstraintsProblemTpl; } // namespace simple ================================================ FILE: src/core/simulator.cpp ================================================ // // Copyright (c) 2022-2024 INRIA // #include "simple/core/simulator.hpp" namespace simple { template struct SimulatorTpl; template void SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, context::Scalar); template void SimulatorTpl::step<::pinocchio::ADMMContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &, context::Scalar); template void SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, context::Scalar); template void SimulatorTpl::step<::pinocchio::PGSContactSolverTpl>( const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const ::pinocchio::container::aligned_vector<::pinocchio::ForceTpl> &, context::Scalar); template void SimulatorTpl::resolveConstraints< ::pinocchio::ADMMContactSolverTpl>(const Eigen::MatrixBase &, const Scalar); template void SimulatorTpl::resolveConstraints< ::pinocchio::PGSContactSolverTpl>(const Eigen::MatrixBase &, const Scalar); } // namespace simple ================================================ FILE: src/empty.cpp ================================================ ================================================ FILE: src/pinocchio_template_instantiation/aba-derivatives.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/fwd.hpp" #include template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void ::pinocchio::computeABADerivatives< ::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>, Eigen::Ref<::simple::context::RowMatrixXs>, Eigen::Ref<::simple::context::RowMatrixXs>, Eigen::Ref<::simple::context::RowMatrixXs>>( const ::simple::context::Model &, ::simple::context::Data &, const Eigen::MatrixBase<::simple::context::VectorXs> &, const Eigen::MatrixBase<::simple::context::VectorXs> &, const Eigen::MatrixBase<::simple::context::VectorXs> &, const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &, const Eigen::MatrixBase> &, const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); ================================================ FILE: src/pinocchio_template_instantiation/aba.cpp ================================================ #include "simple/fwd.hpp" #include template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba< ::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::VectorXs>( const ::simple::context::Model &, ::simple::context::Data &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Convention); template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::VectorXs & ::pinocchio::aba< ::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::VectorXs, ::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>>( const ::simple::context::Model &, ::simple::context::Data &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const Eigen::MatrixBase &, const std::vector<::simple::context::Force, Eigen::aligned_allocator<::simple::context::Force>> &, const Convention); ================================================ FILE: src/pinocchio_template_instantiation/crba.cpp ================================================ #include "simple/fwd.hpp" #include template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const ::simple::context::MatrixXs & ::pinocchio:: crba<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl, ::simple::context::VectorXs>( const context::Model &, context::Data &, const Eigen::MatrixBase &, const Convention convention); ================================================ FILE: src/pinocchio_template_instantiation/joint-model.cpp ================================================ #include "simple/fwd.hpp" #include "pinocchio/multibody/joint/joint-generic.hpp" template struct ::pinocchio::JointModelTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>; template struct ::pinocchio::JointDataTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl>; ================================================ FILE: tests/CMakeLists.txt ================================================ # # Copyright (c) 2024 INRIA # # Find Boost.UnitTestFramework find_package(Boost COMPONENTS unit_test_framework) # Compute flags outside the macro to avoid recomputing it for each tests cxx_flags_by_compiler_frontend(MSVC _USE_MATH_DEFINES OUTPUT TEST_PRIVATE_DEFINITIONS) set(SIMPLE_UNIT_TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}) function(GET_CPP_TEST_NAME name src_dir full_test_name) string(REPLACE "${SIMPLE_UNIT_TEST_DIR}" "" prefix_name "${src_dir}") string(REGEX REPLACE "[/]" "-" prefix_name "${prefix_name}-") set(${full_test_name} "${PROJECT_NAME}-test-cpp${prefix_name}${name}" PARENT_SCOPE) endfunction() function(ADD_SIMPLE_UNIT_TEST name) set(oneValueArgs) set(multiValueArgs PACKAGES) cmake_parse_arguments(unit_test "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) set(PKGS ${unit_test_PACKAGES}) get_cpp_test_name(${name} ${CMAKE_CURRENT_SOURCE_DIR} TEST_NAME) add_unit_test(${TEST_NAME} ${name}.cpp) set(MODULE_NAME "Test") string(REPLACE "-" "_" MODULE_NAME ${MODULE_NAME}) target_compile_definitions( ${TEST_NAME} PRIVATE ${TEST_PRIVATE_DEFINITIONS} BOOST_TEST_DYN_LINK BOOST_TEST_MODULE=${MODULE_NAME} SIMPLE_MODEL_DIR=\"${SIMPLE_MODEL_DIR}\") # There is no RPATH in Windows, then we must use the PATH to find the DLL if(WIN32) string(REPLACE ";" "\\\;" _PATH "$ENV{PATH}") set(ENV_VARIABLES "PATH=${_PATH}\\\;${CMAKE_BINARY_DIR}/src\\\;${CMAKE_BINARY_DIR}/bindings/python/simple") set_tests_properties(${TEST_NAME} PROPERTIES ENVIRONMENT "${ENV_VARIABLES}") endif() set_target_properties(${TEST_NAME} PROPERTIES LINKER_LANGUAGE CXX) target_include_directories(${TEST_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(${TEST_NAME} PUBLIC ${PROJECT_NAME}) # target_link_libraries(${TEST_NAME} PUBLIC Eigen3::Eigen) target_link_libraries(${TEST_NAME} PRIVATE Boost::unit_test_framework) if(PKGS) target_link_libraries(${TEST_NAME} PRIVATE ${PKGS}) endif() endfunction() # CPP test add_subdirectory(forward) # Python tests if(BUILD_PYTHON_INTERFACE) file(GLOB_RECURSE ${PROJECT_NAME}_PYTHON_UNITTEST *.py) foreach(TEST_FILE ${${PROJECT_NAME}_PYTHON_UNITTEST}) get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) string(REGEX REPLACE "${PROJECT_SOURCE_DIR}/" "" TEST_FILE ${TEST_FILE}) add_python_unit_test("${PROJECT_NAME}-test-py-${TEST_NAME}" "${TEST_FILE}" "bindings") endforeach(TEST_FILE ${${PROJECT_NAME}_PYTHON_UNITTEST}) endif(BUILD_PYTHON_INTERFACE) ================================================ FILE: tests/forward/CMakeLists.txt ================================================ add_simple_unit_test(mujoco-humanoid) add_simple_unit_test(simulation-combine-constraints) add_simple_unit_test(simulation-robots) add_simple_unit_test(simulator-minimal) add_simple_unit_test(simulator) # add_simple_unit_test(urdf-romeo) # TODO: TEST - Romeo not found ================================================ FILE: tests/forward/mujoco-humanoid.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/simulator.hpp" #include #include #include #include #include #include "tests/test_data/config.h" #include "../test-utils.hpp" using namespace simple; using namespace pinocchio; using ModelHandle = Simulator::ModelHandle; using DataHandle = Simulator::DataHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; using GeometryDataHandle = Simulator::GeometryDataHandle; #define ADMM ::pinocchio::ADMMContactSolverTpl #define PGS ::pinocchio::PGSContactSolverTpl BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) void addFloorToGeomModel(GeometryModel & geom_model) { using CollisionGeometryPtr = GeometryObject::CollisionGeometryPtr; CollisionGeometryPtr floor_collision_shape = CollisionGeometryPtr(new hpp::fcl::Halfspace(0.0, 0.0, 1.0, 0.0)); const SE3 M = SE3::Identity(); GeometryObject floor("floor", 0, 0, M, floor_collision_shape); geom_model.addGeometryObject(floor); } void addSystemCollisionPairs(const Model & model, GeometryModel & geom_model, const Eigen::VectorXd & qref) { Data data(model); GeometryData geom_data(geom_model); // TI this function to gain compilation speed on this test ::pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, qref); geom_model.removeAllCollisionPairs(); for (std::size_t i = 0; i < geom_model.geometryObjects.size(); ++i) { for (std::size_t j = i; j < geom_model.geometryObjects.size(); ++j) { if (i == j) { continue; // don't add collision pair if same object } const GeometryObject & gobj_i = geom_model.geometryObjects[i]; const GeometryObject & gobj_j = geom_model.geometryObjects[j]; if (gobj_i.name == "floor" || gobj_j.name == "floor") { // if floor, always add a collision pair geom_model.addCollisionPair(CollisionPair(i, j)); } else { if (gobj_i.parentJoint == gobj_j.parentJoint) { // don't add collision pair if same parent continue; } // run collision detection -- add collision pair if shapes are not colliding const SE3 M1 = geom_data.oMg[i]; const SE3 M2 = geom_data.oMg[j]; hpp::fcl::CollisionRequest colreq; colreq.security_margin = 1e-2; // 1cm of clearance hpp::fcl::CollisionResult colres; hpp::fcl::collide( gobj_i.geometry.get(), ::pinocchio::toFclTransform3f(M1), // gobj_j.geometry.get(), ::pinocchio::toFclTransform3f(M2), // colreq, colres); if (!colres.isCollision()) { geom_model.addCollisionPair(CollisionPair(i, j)); } } } } } BOOST_AUTO_TEST_CASE(mujoco_humanoid) { ModelHandle model_handle(new Model()); Model & model = ::pinocchio::helper::get_ref(model_handle); GeometryModelHandle geom_model_handle(new GeometryModel()); GeometryModel & geom_model = ::pinocchio::helper::get_ref(geom_model_handle); const bool verbose = false; ::pinocchio::mjcf::buildModel(SIMPLE_TEST_DATA_DIR "/mujoco_humanoid.xml", model, verbose); ::pinocchio::mjcf::buildGeom(model, SIMPLE_TEST_DATA_DIR "/mujoco_humanoid.xml", pinocchio::COLLISION, geom_model); addFloorToGeomModel(geom_model); // sanity checks assert(model.nv == 27); assert(geom_model.geometryObjects.size() == 20); // initial state const Eigen::VectorXd q0 = model.referenceConfigurations["qpos0"]; const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv); const Eigen::VectorXd tau0 = Eigen::VectorXd::Zero(model.nv); // add collision pairs addSystemCollisionPairs(model, geom_model, q0); assert(geom_model.collisionPairs.size() == 175); // run simulation model.lowerPositionLimit.setConstant(-std::numeric_limits::infinity()); model.upperPositionLimit.setConstant(std::numeric_limits::infinity()); model.lowerDryFrictionLimit.setZero(); model.upperDryFrictionLimit.setZero(); const double dt = 1e-3; const Eigen::VectorXd zero_torque = Eigen::VectorXd::Zero(model.nv); { Simulator sim(model_handle, geom_model_handle); Eigen::VectorXd q = q0; Eigen::VectorXd v = v0; for (size_t i = 0; i < 100; ++i) { BOOST_CHECK_NO_THROW(sim.step(q, v, zero_torque, dt)); BOOST_CHECK(sim.admm_constraint_solver.getIterationCount() < sim.admm_constraint_solver_settings.max_iter); BOOST_CHECK(sim.pgs_constraint_solver.getIterationCount() == 0); // make sure pgs didnt run q = sim.qnew; v = sim.vnew; } } { Simulator sim(model_handle, geom_model_handle); Eigen::VectorXd q = q0; Eigen::VectorXd v = v0; for (size_t i = 0; i < 100; ++i) { BOOST_CHECK_NO_THROW(sim.step(q, v, zero_torque, dt)); BOOST_CHECK(sim.pgs_constraint_solver.getIterationCount() < sim.pgs_constraint_solver_settings.max_iter); BOOST_CHECK(sim.admm_constraint_solver.getIterationCount() == 0); // make sure admm didnt run q = sim.qnew; v = sim.vnew; } } } BOOST_AUTO_TEST_SUITE_END() ================================================ FILE: tests/forward/simulation-combine-constraints.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/simulator.hpp" #include #include #include #include #include #include #include #include #include #include "tests/test_data/config.h" #include "../test-utils.hpp" using namespace simple; using namespace pinocchio; using ModelHandle = Simulator::ModelHandle; using DataHandle = Simulator::DataHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; using GeometryDataHandle = Simulator::GeometryDataHandle; using BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel; using BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector; using WeldConstraintModel = Simulator::WeldConstraintModel; using WeldConstraintModelVector = Simulator::WeldConstraintModelVector; #define ADMM ::pinocchio::ADMMContactSolverTpl using ModelHandle = Simulator::ModelHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; /// \brief Creates a scene of 10 balls, all lying on a floor. /// \param reduced_collision_pairs activate only the collision pair between balls and floor. std::tuple createBallsScene(bool prismatic_joint, bool collision_plane, bool inter_colision) { Model model; GeometryModel geom_model; // Add plane hpp::fcl::CollisionGeometryPtr_t plane_ptr(new hpp::fcl::Halfspace(0., 0., 1., 0.)); const GeometryObject plane_geom = GeometryObject("plane", 0, SE3::Identity(), plane_ptr); GeomIndex plane_id = geom_model.addGeometryObject(plane_geom); // Add balls const std::size_t nballs = 10; const double radius = 1.0; const double mass = 1.0; for (std::size_t i = 0; i < nballs; ++i) { const std::string name = "ball_" + std::to_string(i); const std::string joint_name = "joint_" + std::to_string(i); const std::string frame_name = "frame_" + std::to_string(i); const Inertia inertia = Inertia::FromSphere(mass, radius); const JointIndex parent = 0; JointIndex joint_id; SE3 placement = SE3::Identity(); ; if (prismatic_joint) { placement.translation() = Eigen::Vector3d(static_cast(i) * 2 * radius * 1.1, 0, 0); const JointModelPZ joint = JointModelPZ(); joint_id = model.addJoint(parent, joint, placement, joint_name); } else { const JointModelFreeFlyer joint = JointModelFreeFlyer(); joint_id = model.addJoint(parent, joint, placement, joint_name); } model.appendBodyToJoint(joint_id, inertia); hpp::fcl::CollisionGeometryPtr_t ball_ptr(new hpp::fcl::Sphere(radius)); const GeometryObject ball_geom = GeometryObject(name, joint_id, placement, ball_ptr); GeomIndex ball_id = geom_model.addGeometryObject(ball_geom); if (collision_plane) { CollisionPair col_pair(plane_id, ball_id); geom_model.addCollisionPair(col_pair); } } if (inter_colision) { for (size_t i = 0; i < geom_model.geometryObjects.size(); ++i) { for (size_t j = i + 1; j < geom_model.geometryObjects.size(); ++j) { CollisionPair col_pair(i, j); geom_model.addCollisionPair(col_pair); } } } Eigen::VectorXd q0 = neutral(model); for (int i = 0; i < (int)(nballs); ++i) { int nq = prismatic_joint ? 1 : 7; if (prismatic_joint) { q0(i * nq) = radius; } else { q0(i * nq) = static_cast(i) * 2 * radius * 1.1; q0(i * nq + 2) = radius; } } return { std::make_shared(model), std::make_shared(geom_model), q0, }; } void compareConstraintsProblems(const ConstraintsProblem & problem1, const ConstraintsProblem & problem2, const double tol) { const std::size_t num_contacts = problem1.getNumberOfContacts(); INDEX_EQUALITY_CHECK(num_contacts, problem2.getNumberOfContacts()); if (num_contacts != problem2.getNumberOfContacts()) { PINOCCHIO_THROW_PRETTY(std::runtime_error, "Cannot compare contact problems; They don't have the same amount of contacts."); } EIGEN_VECTOR_IS_APPROX(problem1.g(), problem2.g(), tol); // Need to compare the constraints cholesky up to `tol` // BOOST_CHECK(problem1.constraint_cholesky_decomposition == problem2.constraint_cholesky_decomposition); EIGEN_VECTOR_IS_APPROX(problem1.frictional_point_constraints_forces(), problem2.frictional_point_constraints_forces(), tol); for (std::size_t i = 0; i < num_contacts; ++i) { const Simulator::ConstraintModel & cmodel1 = problem1.constraint_models[i]; const Simulator::ConstraintModel & cmodel2 = problem2.constraint_models[i]; BOOST_CHECK(cmodel1 == cmodel2); BOOST_CHECK(problem1.previous_colliding_collision_pairs[i] == problem2.previous_colliding_collision_pairs[i]); } } double rand_interval(double rmin, double rmax) { double val = rand() / (static_cast(RAND_MAX) + 1); return (val * (rmax - rmin) + rmin); } // --------------------------------------------------------------------------------------------------- // Actual tests // --------------------------------------------------------------------------------------------------- BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) // --------------------------------------------------------------------------------------------------- // ----- Test stating scene // Given an initial config that should be static test that the system is not moving. void test_static_scene(bool prismatic_joint) { // Get system with vertical prismatic, collision with floor and no inter collision std::tuple system = createBallsScene(prismatic_joint, true, false); ModelHandle model = std::get<0>(system); GeometryModelHandle geom_model = std::get<1>(system); const Eigen::VectorXd & q0 = std::get<2>(system); // Initial state Eigen::VectorXd q = q0; Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); // ... with this initial state, the balls should not move Simulator sim(model, geom_model); sim.admm_constraint_solver_settings.absolute_precision = 1e-12; sim.admm_constraint_solver_settings.relative_precision = 1e-12; const double dt = 1e-3; sim.step(q, v, tau, dt); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), (model->njoints - 1) * (prismatic_joint ? 1 : 6)); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); const std::size_t num_contacts = sim.constraints_problem().getNumberOfContacts(); INDEX_EQUALITY_CHECK(num_contacts, sim.geom_model().collisionPairs.size()); BOOST_CHECK(sim.constraints_problem().check()); BOOST_CHECK(sim.checkCollisionPairs()); INDEX_EQUALITY_CHECK(sim.constraints_problem().g().size(), static_cast(3 * num_contacts)); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-6); EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-6); // Calling a second time step to test warmstart - contact solver should immediatly converge sim.step(q, v, tau, dt); // TODO: TEST - It gives 2 instead of 0 :( Warmstart must be solved INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 2); // Running for 5 timesteps const std::size_t horizon = 5; q = q0; v.setZero(); for (std::size_t i = 0; i < horizon; ++i) { sim.step(q, v, tau, dt); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); INDEX_EQUALITY_CHECK(sim.geom_model().collisionPairs.size(), sim.constraints_problem().constraint_models.size()); const std::size_t num_contacts = sim.constraints_problem().getNumberOfContacts(); INDEX_EQUALITY_CHECK(sim.geom_model().collisionPairs.size(), num_contacts); BOOST_CHECK(sim.constraints_problem().check()); INDEX_EQUALITY_CHECK(sim.constraints_problem().g().size(), static_cast(3 * num_contacts)); REAL_IS_APPROX(sim.vnew.norm(), 0.0, 1e-7); EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-8); EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-8); ConstraintsProblem problem1 = sim.constraints_problem(); // Calling a second time step to test warmstart - contact solver should immediatly converge sim.step(q, v, tau, dt); // TODO: TEST - It gives 3 instead of 0 :( Warmstart must be solved INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 3); INDEX_EQUALITY_CHECK(sim.constraints_problem().getNumberOfContacts(), num_contacts); ConstraintsProblem problem2 = sim.constraints_problem(); compareConstraintsProblems(problem1, problem2, 1e-5); // Update state q = sim.qnew; v = sim.vnew; } } BOOST_AUTO_TEST_CASE(simulator_instance_static_balls_using_joint) { test_static_scene(true); } BOOST_AUTO_TEST_CASE(simulator_instance_static_balls_using_freeflyer) { test_static_scene(false); } // --------------------------------------------------------------------------------------------------- // ----- Test moving scene // Test if the simuation gives the same trajectory with two different simulator constructed differently // and eventually with one using warmstart and other one do not void test_moving_scene(bool prismatic_joint, bool compare_reset, bool inter_collision) { // Get system std::tuple system = createBallsScene(prismatic_joint, true, inter_collision); ModelHandle model = std::get<0>(system); GeometryModelHandle geom_model = std::get<1>(system); Eigen::VectorXd q0 = std::get<2>(system); Data _data(*model); std::shared_ptr data = std::make_shared(_data); GeometryData _geom_data(*geom_model); std::shared_ptr geom_data = std::make_shared(_geom_data); // Initial state const std::size_t nballs = geom_model->geometryObjects.size() - 1; for (Eigen::Index i = 0; i < static_cast(nballs); ++i) { Eigen::Index idx(prismatic_joint ? i : i * 7 + 2); q0(idx) += rand_interval(0.0, 1.0); } Eigen::VectorXd v0 = Eigen::VectorXd::Random(model->nv); std::array sims{Simulator(model, geom_model), Simulator(model, data, geom_model, geom_data)}; // Running for 5000 timesteps, twice const std::size_t horizon = 5; const double dt = 1e-3; std::array, 2> qs; std::array, 2> vs; std::array, 2> vfrees; std::array, 2> constraints_problems; std::array, 2> numits; for (std::size_t i = 0; i < qs.size(); ++i) { Eigen::VectorXd q = q0; Eigen::VectorXd v = v0; Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); qs[i].reserve(horizon); vs[i].reserve(horizon); vfrees[i].reserve(horizon); constraints_problems[i].reserve(horizon); numits[i].reserve(horizon); Simulator & sim = sims[i]; if (compare_reset && i == 1) { sim.reset(); } for (std::size_t t = 0; t < horizon; ++t) { sim.step(q, v, tau, dt); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); BOOST_CHECK(sim.constraints_problem().check()); // Update state q = sim.qnew; v = sim.vnew; qs[i].emplace_back(q); vs[i].emplace_back(v); vfrees[i].emplace_back(sim.vfree); constraints_problems[i].emplace_back(sim.constraints_problem()); numits[i].emplace_back(sim.admm_constraint_solver.getIterationCount()); } } // Check that trajectories are identical for (std::size_t i = 0; i < horizon; ++i) { const double tol = 1e-10; EIGEN_VECTOR_IS_APPROX(qs[0][i], qs[1][i], tol); EIGEN_VECTOR_IS_APPROX(vs[0][i], vs[1][i], tol); EIGEN_VECTOR_IS_APPROX(vfrees[0][i], vfrees[1][i], tol); compareConstraintsProblems(constraints_problems[0][i], constraints_problems[1][i], tol); INDEX_EQUALITY_CHECK(numits[0][i], numits[1][i]); } } BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_joint) { test_moving_scene(true, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_joint_compare_warmstart) { test_moving_scene(true, true, false); } BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer) { test_moving_scene(false, false, true); } BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer_compare_warmstart) { test_moving_scene(false, true, true); } BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer_nointercollision) { test_moving_scene(false, false, false); } // --------------------------------------------------------------------------------------------------- // ----- Test one step with different combination of constraints // Test if one step give the expected result and the right constraint problem size void test_constraint_scene(bool prismatic_joint, bool floor_contact, bool bilat, bool weld, bool friction, bool limit, bool rand) { // Get system std::tuple system = createBallsScene(prismatic_joint, floor_contact, false); ModelHandle model = std::get<0>(system); GeometryModelHandle geom_model = std::get<1>(system); Eigen::VectorXd & q0 = std::get<2>(system); const std::size_t nballs = (std::size_t)(model->njoints - 1); DataHandle data = std::make_shared(*model); // Add joint constraints // Quantities for joint constraints // TODO: TEST - Update when nq>1 is available bool really_friction = friction && prismatic_joint; bool really_limit = limit && prismatic_joint; std::size_t n_active_limits = 0; const std::size_t n_constrained_v = nballs; if (really_friction) { model->lowerDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * -1.0; model->upperDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * 1.0; } if (really_limit) { model->lowerPositionLimit = Eigen::VectorXd::Ones(model->nv) * -100000.0; model->upperPositionLimit = Eigen::VectorXd::Ones(model->nv) * 100000.0; } // Initial state Eigen::VectorXd q = q0; Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); if (rand) { for (Eigen::Index i = 0; i < static_cast(nballs); ++i) { Eigen::Index idx(prismatic_joint ? i : i * 7 + 2); q[idx] += rand_interval(0.0, 1.0); if (q[idx] - model->positionLimitMargin[idx] <= model->lowerPositionLimit(idx)) { n_active_limits++; } if (q[idx] + model->positionLimitMargin[idx] >= model->upperPositionLimit(idx)) { n_active_limits++; } } v = Eigen::VectorXd::Random(model->nv); } // Adding one attach constraint for each ball if required pinocchio::forwardKinematics(*model, *data, q); WeldConstraintModelVector weld_constraint_models; BilateralPointConstraintModelVector bilateral_constraint_models; std::size_t end_bilat = 0; std::size_t start_weld = nballs; std::size_t attach_constraint_size = 0; if (bilat && weld) { // Half of balls are welded, other are point attached end_bilat = nballs / 2; start_weld = nballs / 2; attach_constraint_size = (nballs / 2) * 3 + (nballs - nballs / 2) * 6; } else if (bilat && !weld) { attach_constraint_size = nballs * 3; end_bilat = nballs; } else if (!bilat && weld) { attach_constraint_size = nballs * 6; start_weld = 0; } else if (!bilat && !weld) { attach_constraint_size = 0; } for (std::size_t i = 0; i < end_bilat; ++i) { const std::string name = "joint_" + std::to_string(i); const JointIndex joint1_id = model->getJointId(name); const GeomIndex joint2_id = 0; const SE3 joint1_placement = SE3::Identity(); const SE3 joint2_placement = data->oMi[joint1_id]; bilateral_constraint_models.push_back(BilateralPointConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement)); } for (std::size_t i = start_weld; i < nballs; ++i) { const std::string name = "joint_" + std::to_string(i); const JointIndex joint1_id = model->getJointId(name); const GeomIndex joint2_id = 0; const SE3 joint1_placement = SE3::Identity(); const SE3 joint2_placement = data->oMi[joint1_id]; weld_constraint_models.push_back(WeldConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement)); } // Creating the simulator and make a step Simulator sim(model, geom_model, bilateral_constraint_models, weld_constraint_models); const double dt = 1e-3; sim.step(q, v, tau, dt); // Test that if balls are on the floor there is 1 contact by ball bool is_laying = !rand && floor_contact; std::size_t ncontacts = 0; if (is_laying) { ncontacts = nballs; INDEX_EQUALITY_CHECK(ncontacts, sim.constraints_problem().getNumberOfContacts()); } // Test if that ball should not move the new value are 0 bool must_not_move = is_laying || (!rand && bilat) || (weld && !bilat); if (must_not_move) { // TODO: TEST - Here precision of 1e-5 is required.. Maybe lack of precision ? EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-5); EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-5); if (!rand) { EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-5); } } // Test the constraint problem size INDEX_EQUALITY_CHECK( sim.constraints_problem().constraints_problem_size(), (int)(3 * ncontacts + attach_constraint_size + really_friction * n_constrained_v + really_limit * n_active_limits)); // calling the simulator again should not change the state // and the solver should converge in one iteration if (must_not_move && !rand) { sim.step(q, v, tau, dt); // TODO: TEST - It gives many different results instead of 0 :( Warmstart must be solved // Uperbound bound 20 is required INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 20); } } // test_constraint_scene( // prismatic_joint, // floor_contact, // bilat, // weld, // friction, // limit, // rand // ) BOOST_AUTO_TEST_CASE(simulator_instance_step_with_no_constraint) { test_constraint_scene(false, true, false, false, false, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_bilateral_constraint) { test_constraint_scene(false, false, true, false, false, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_constraint) { test_constraint_scene(false, false, false, true, false, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_constraint_ill_defined) { test_constraint_scene(false, false, false, true, false, false, true); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_and_bilat_constraint) { test_constraint_scene(false, false, true, true, false, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_limit) { test_constraint_scene(true, false, false, false, false, true, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction) { test_constraint_scene(true, false, false, false, true, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_limit_static) { test_constraint_scene(true, true, false, false, false, true, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_static) { test_constraint_scene(true, true, false, false, true, false, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_joints_constraint_static) { test_constraint_scene(true, true, false, false, true, true, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints) { test_constraint_scene(true, false, true, true, true, true, true); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_static) { test_constraint_scene(true, false, true, true, true, true, false); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_and_contact) { test_constraint_scene(true, true, true, true, true, true, true); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_and_contact_static) { test_constraint_scene(true, true, true, true, true, true, false); } // Loop on all combination BOOST_AUTO_TEST_CASE(simulator_instance_step_test_all_combination) { for (std::size_t i = 0; i < 128; ++i) { bool prismatic_joint = i & 0x01; bool floor_contact = (i >> 1) & 0x01; bool bilat = (i >> 2) & 0x01; bool weld = (i >> 3) & 0x01; bool friction = (i >> 4) & 0x01; bool limit = (i >> 5) & 0x01; bool rand = (i >> 6) & 0x01; test_constraint_scene(prismatic_joint, floor_contact, bilat, weld, friction, limit, rand); } } BOOST_AUTO_TEST_SUITE_END() ================================================ FILE: tests/forward/simulation-robots.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/simulator.hpp" #include #include #include #include #include #include #include #include #include #include "tests/test_data/config.h" #include "../test-utils.hpp" using namespace simple; using namespace pinocchio; using ModelHandle = Simulator::ModelHandle; using DataHandle = Simulator::DataHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; using GeometryDataHandle = Simulator::GeometryDataHandle; using BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel; using BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector; using WeldConstraintModel = Simulator::WeldConstraintModel; using WeldConstraintModelVector = Simulator::WeldConstraintModelVector; #define ADMM ::pinocchio::ADMMContactSolverTpl using ModelHandle = Simulator::ModelHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint) { // Construct humanoid ModelHandle model(new Model()); buildModels::humanoid(*model, true); model->lowerPositionLimit = Eigen::VectorXd::Constant(model->nq, -1.0) * std::numeric_limits::max(); model->upperPositionLimit = Eigen::VectorXd::Constant(model->nq, 1.0) * std::numeric_limits::max(); DataHandle data = std::make_shared(*model); GeometryModelHandle geom_model(new GeometryModel()); // Initial state Eigen::VectorXd q = pinocchio::neutral(*model); // Bilateral constraint on the robot right wrist BilateralPointConstraintModelVector bilateral_constraint_models; pinocchio::framesForwardKinematics(*model, *data, q); const JointIndex joint1_id = 0; const GeomIndex joint2_id = 13; ::pinocchio::SE3 Mc = data->oMi[joint2_id]; const SE3 joint1_placement = Mc; const SE3 joint2_placement = SE3::Identity(); bilateral_constraint_models.push_back(BilateralPointConstraintModel(*model, joint1_id, joint1_placement, joint2_id, joint2_placement)); bilateral_constraint_models[0].baumgarte_corrector_parameters().Kp = 0.1; // The humanoid's freeflyer's height should remain the same Simulator sim(model, geom_model, bilateral_constraint_models); sim.admm_constraint_solver_settings.absolute_precision = 1e-9; sim.admm_constraint_solver_settings.relative_precision = 1e-9; sim.admm_constraint_solver_settings.max_iter = 1000; const double dt = 1e-3; Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt)); pinocchio::framesForwardKinematics(*model, *data, sim.qnew); EIGEN_VECTOR_IS_APPROX(Mc.translation(), data->oMi[joint2_id].translation(), 1e-3); // Calling the simulator twice to test warmstart sim.step(q, v, tau, dt); INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0); for (int i = 0; i < 10; ++i) { Eigen::VectorXd q = sim.qnew; Eigen::VectorXd v = sim.vnew; BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt)); pinocchio::framesForwardKinematics(*model, *data, sim.qnew); EIGEN_VECTOR_IS_APPROX(Mc.translation(), data->oMi[joint2_id].translation(), 1e-3); } } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_on_joints) { ModelHandle model(new Model()); buildModels::manipulator(*model); model->lowerDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * -100000.0; model->upperDryFrictionLimit = Eigen::VectorXd::Ones(model->nv) * 100000.0; GeometryModelHandle geom_model(new GeometryModel()); buildModels::manipulatorGeometries(*model, *geom_model); Simulator sim(model, geom_model); Eigen::VectorXd q = neutral(*model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); const double dt = 1e-3; sim.step(q, v, tau, dt); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); // calling the simulator again should not change the state // and the solver should converge in one iteration sim.step(q, v, tau, dt); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); EIGEN_VECTOR_IS_APPROX(sim.vnew, v, 1e-6); EIGEN_VECTOR_IS_APPROX(sim.qnew, q, 1e-6); INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0); } BOOST_AUTO_TEST_CASE(simulator_instance_step_with_limits_on_joints_for_manipulator) { ModelHandle model(new Model()); buildModels::manipulator(*model); // We first consider not active limits model->lowerPositionLimit = Eigen::VectorXd::Ones(model->nq) * -100000.0; model->upperPositionLimit = Eigen::VectorXd::Ones(model->nq) * 100000.0; GeometryModelHandle geom_model(new GeometryModel()); buildModels::manipulatorGeometries(*model, *geom_model); Simulator sim(model, geom_model); Eigen::VectorXd q = neutral(*model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); const double dt = 1e-3; sim.step(q, v, tau, dt); // simulated velocity should be the same as the one computed with aba as constraints are not active Eigen::VectorXd vnew = v + dt * pinocchio::aba(*model, sim.data(), q, v, tau); EIGEN_VECTOR_IS_APPROX(sim.vnew, vnew, 1e-6); // calling the simulator again should not change the state // and the solver should converge in one iteration sim.step(q, v, tau, dt); EIGEN_VECTOR_IS_APPROX(sim.vnew, vnew, 1e-6); INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0); // we now consider active limits q = Eigen::VectorXd::Zero(model->nv); model->lowerPositionLimit = Eigen::VectorXd::Zero(model->nv); model->upperPositionLimit = Eigen::VectorXd::Zero(model->nv); sim = Simulator(model, geom_model); sim.admm_constraint_solver_settings.absolute_precision = 1e-9; sim.admm_constraint_solver_settings.relative_precision = 1e-9; sim.step(q, v, tau, dt); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); // calling the simulator again should not change the state // and the solver should converge in one iteration sim.step(q, v, tau, dt); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); INDEX_EQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 0); } BOOST_AUTO_TEST_SUITE_END() ================================================ FILE: tests/forward/simulator-minimal.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/simulator.hpp" #include #include #include #include #include #include #include #include #include #include "tests/test_data/config.h" #include "../test-utils.hpp" using namespace simple; using namespace pinocchio; using BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel; using BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector; using WeldConstraintModel = Simulator::WeldConstraintModel; using WeldConstraintModelVector = Simulator::WeldConstraintModelVector; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint_minimal) { // Construct humanoid Model model; buildModels::humanoid(model, true); Data data(model); // Initial state Eigen::VectorXd q = pinocchio::neutral(model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); const double dt = 1e-3; // Compute vfree and crba for G and g crba(model, data, q, Convention::WORLD); const Eigen::VectorXd v_free = dt * aba(model, data, q, v, tau, Convention::WORLD); // Bilateral constraint on the robot right wrist pinocchio::forwardKinematics(model, data, q); const JointIndex joint1_id = 0; const GeomIndex joint2_id = 14; assert((int)joint2_id < model.njoints); assert(model.nvs[joint2_id] == 1); // make sure its a bilaterable joint ::pinocchio::SE3 Mc = data.oMi[joint2_id]; const SE3 joint1_placement = Mc; const SE3 joint2_placement = SE3::Identity(); using ConstraintModel = BilateralPointConstraintModel; using ConstraintData = typename ConstraintModel::ConstraintData; std::vector constraint_models; std::vector constraint_datas; ConstraintModel cm(model, joint1_id, joint1_placement, joint2_id, joint2_placement); constraint_models.push_back(cm); constraint_datas.push_back(cm.createData()); const Eigen::DenseIndex constraint_size = getTotalConstraintSize(constraint_models); // Delassus ContactCholeskyDecomposition chol(model, constraint_models); chol.compute(model, data, constraint_models, constraint_datas, 1e-10); // Solve constraint const Eigen::MatrixXd delassus = chol.getDelassusCholeskyExpression().matrix(); const DelassusOperatorDense G(delassus); Eigen::MatrixXd constraint_jacobian(delassus.rows(), model.nv); constraint_jacobian.setZero(); getConstraintsJacobian(model, data, constraint_models, constraint_datas, constraint_jacobian); const Eigen::VectorXd g = constraint_jacobian * v_free; PGSContactSolver pgs_solver(int(delassus.rows())); pgs_solver.setAbsolutePrecision(1e-10); pgs_solver.setRelativePrecision(1e-14); pgs_solver.setMaxIterations(1000); Eigen::VectorXd primal_solution = Eigen::VectorXd::Zero(constraint_size); const bool has_converged = pgs_solver.solve(G, g, constraint_models, dt, boost::make_optional((Eigen::Ref)primal_solution)); BOOST_CHECK(has_converged); primal_solution = pgs_solver.getPrimalSolution(); const Eigen::VectorXd tau_ext = constraint_jacobian.transpose() * primal_solution / dt; Eigen::VectorXd v_next = v + dt * aba(model, data, q, v, (tau + tau_ext).eval(), Convention::WORLD); Eigen::VectorXd dv = v * dt; Eigen::VectorXd q_next = ::pinocchio::integrate(model, q, dv); Eigen::VectorXd v_wrist_expected = Eigen::VectorXd::Zero(model.nvs[joint2_id]); EIGEN_VECTOR_IS_APPROX( v_next.segment(model.idx_vs[joint2_id], model.nvs[joint2_id]), // v_wrist_expected, // 1e-6); pinocchio::forwardKinematics(model, data, q_next); EIGEN_VECTOR_IS_APPROX(Mc.translation(), data.oMi[joint2_id].translation(), 1e-6); } BOOST_AUTO_TEST_CASE(humanoid_with_weld_constraint_minimal) { // Construct humanoid Model model; buildModels::humanoid(model, true); Data data(model); // Initial state Eigen::VectorXd q = pinocchio::neutral(model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); const double dt = 1e-3; // Compute vfree and crba for G and g crba(model, data, q, Convention::WORLD); const Eigen::VectorXd v_free = dt * aba(model, data, q, v, tau, Convention::WORLD); // Bilateral constraint on the robot right wrist pinocchio::forwardKinematics(model, data, q); const JointIndex joint1_id = 0; const GeomIndex joint2_id = 14; assert((int)joint2_id < model.njoints); assert(model.nvs[joint2_id] == 1); // make sure its a bilaterable joint ::pinocchio::SE3 Mc = data.oMi[joint2_id]; const SE3 joint1_placement = Mc; const SE3 joint2_placement = SE3::Identity(); using ConstraintModel = BilateralPointConstraintModel; using ConstraintData = typename ConstraintModel::ConstraintData; std::vector constraint_models; std::vector constraint_datas; ConstraintModel cm(model, joint1_id, joint1_placement, joint2_id, joint2_placement); constraint_models.push_back(cm); constraint_datas.push_back(cm.createData()); const Eigen::DenseIndex constraint_size = getTotalConstraintSize(constraint_models); // Delassus ContactCholeskyDecomposition chol(model, constraint_models); chol.compute(model, data, constraint_models, constraint_datas, 1e-10); // Solve constraint const Eigen::MatrixXd delassus = chol.getDelassusCholeskyExpression().matrix(); const DelassusOperatorDense G(delassus); Eigen::MatrixXd constraint_jacobian(delassus.rows(), model.nv); constraint_jacobian.setZero(); getConstraintsJacobian(model, data, constraint_models, constraint_datas, constraint_jacobian); const Eigen::VectorXd g = constraint_jacobian * v_free; PGSContactSolver pgs_solver(int(delassus.rows())); pgs_solver.setAbsolutePrecision(1e-10); pgs_solver.setRelativePrecision(1e-14); pgs_solver.setMaxIterations(1000); Eigen::VectorXd primal_solution = Eigen::VectorXd::Zero(constraint_size); const bool has_converged = pgs_solver.solve(G, g, constraint_models, dt, boost::make_optional((Eigen::Ref)primal_solution)); BOOST_CHECK(has_converged); primal_solution = pgs_solver.getPrimalSolution(); const Eigen::VectorXd tau_ext = constraint_jacobian.transpose() * primal_solution / dt; Eigen::VectorXd v_next = v + dt * aba(model, data, q, v, (tau + tau_ext).eval(), Convention::WORLD); Eigen::VectorXd dv = v * dt; Eigen::VectorXd q_next = ::pinocchio::integrate(model, q, dv); Eigen::VectorXd v_wrist_expected = Eigen::VectorXd::Zero(model.nvs[joint2_id]); EIGEN_VECTOR_IS_APPROX( v_next.segment(model.idx_vs[joint2_id], model.nvs[joint2_id]), // v_wrist_expected, // 1e-6); pinocchio::forwardKinematics(model, data, q_next); EIGEN_VECTOR_IS_APPROX(Mc.translation(), data.oMi[joint2_id].translation(), 1e-6); } BOOST_AUTO_TEST_SUITE_END() ================================================ FILE: tests/forward/simulator.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/simulator.hpp" #include #include #include #include #include #include #include #include #include #include "tests/test_data/config.h" #include "../test-utils.hpp" using namespace simple; using namespace pinocchio; using ModelHandle = Simulator::ModelHandle; using DataHandle = Simulator::DataHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; using GeometryDataHandle = Simulator::GeometryDataHandle; using BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel; using BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector; using WeldConstraintModel = Simulator::WeldConstraintModel; using WeldConstraintModelVector = Simulator::WeldConstraintModelVector; #define ADMM ::pinocchio::ADMMContactSolverTpl using ModelHandle = Simulator::ModelHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(simulator_instance_constructor) { ModelHandle model(new Model()); DataHandle data(new Data(*model)); GeometryModelHandle geom_model(new GeometryModel()); GeometryDataHandle geom_data(new GeometryData(*geom_model)); BOOST_CHECK_NO_THROW(Simulator sim(model, data, geom_model, geom_data)); } BOOST_AUTO_TEST_CASE(simulator_instance_constructor_2) { ModelHandle model(new Model()); GeometryModelHandle geom_model(new GeometryModel()); BOOST_CHECK_NO_THROW(Simulator sim(model, geom_model)); } BOOST_AUTO_TEST_CASE(simulator_instance_step) { ModelHandle model(new Model()); buildModels::humanoidRandom(*model); buildModels::manipulator(*model); GeometryModelHandle geom_model(new GeometryModel()); buildModels::manipulatorGeometries(*model, *geom_model); Simulator sim(model, geom_model); Eigen::VectorXd q = neutral(*model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); const double dt = 1e-3; BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt)); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); } BOOST_AUTO_TEST_CASE(simulator_instance_step_make_shared) { Model _model; buildModels::manipulator(_model); ModelHandle model = std::make_shared(_model); GeometryModel _geom_model; buildModels::manipulatorGeometries(_model, _geom_model); GeometryModelHandle geom_model = std::make_shared(_geom_model); Simulator sim(model, geom_model); Eigen::VectorXd q = neutral(*model); Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); const double dt = 1e-3; BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt)); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); } BOOST_AUTO_TEST_CASE(simulator_instance_step_cubes) { const int N_cubes = 10; std::vector radius(N_cubes, 1.0); Model _model; GeometryModel _geom_model; hpp::fcl::CollisionGeometryPtr_t plane_ptr(new hpp::fcl::Halfspace(0., 0., 1., 0.)); const FrameIndex plane_frame = FrameIndex(0); const GeometryObject plane_geom = GeometryObject("plane", 0, plane_frame, SE3::Identity(), plane_ptr); GeomIndex plane_id = _geom_model.addGeometryObject(plane_geom); for (int i = 0; i < N_cubes; ++i) { const std::string name = "cube_" + std::to_string(i); const std::string joint_name = "joint_" + std::to_string(i); const std::string frame_name = "frame_" + std::to_string(i); const double mass = 1.0; const double r = radius[static_cast(i)]; const Inertia inertia = Inertia::FromBox(mass, r, r, r); const JointModelFreeFlyer joint = JointModelFreeFlyer(); const JointIndex parent = 0; const SE3 placement = SE3::Identity(); JointIndex joint_id = _model.addJoint(parent, joint, placement, joint_name); _model.appendBodyToJoint(joint_id, inertia); hpp::fcl::CollisionGeometryPtr_t box_ptr(new hpp::fcl::Box(r, r, r)); const GeometryObject box_geom = GeometryObject(name, joint_id, placement, box_ptr); GeomIndex box_id = _geom_model.addGeometryObject(box_geom); CollisionPair cp(plane_id, box_id); _geom_model.addCollisionPair(cp); } std::shared_ptr model = std::make_shared(_model); std::shared_ptr geom_model = std::make_shared(_geom_model); Simulator sim(model, geom_model); sim.admm_constraint_solver_settings.absolute_precision = 1e-12; sim.admm_constraint_solver_settings.relative_precision = 1e-12; Eigen::VectorXd q = neutral(*model); for (int i = 0; i < N_cubes; ++i) { q(i * 7) = static_cast(i) * radius[static_cast(i)] * 1.1; q(i * 7 + 2) = radius[static_cast(i)] / 2.; } Eigen::VectorXd v = Eigen::VectorXd::Zero(model->nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(model->nv); const double dt = 1e-3; sim.step(q, v, tau, dt); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); // 4 contact points (cube-plane) INDEX_EQUALITY_CHECK(4 * N_cubes, sim.constraints_problem().constraint_models.size()); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); // Calling a second time step to test warmstart q = sim.qnew; v = sim.vnew; sim.step(q, v, tau, dt); // TODO: TEST - It gives 2 instead of 0 :( Warmstart must be solved INDEX_INEQUALITY_CHECK(sim.admm_constraint_solver.getIterationCount(), 2); INDEX_EQUALITY_CHECK(sim.vfree.size(), model->nv); INDEX_EQUALITY_CHECK(sim.vnew.size(), model->nv); INDEX_EQUALITY_CHECK(sim.ftotal.size(), static_cast(model->njoints)); INDEX_EQUALITY_CHECK(4 * N_cubes, sim.constraints_problem().constraint_models.size()); EIGEN_VECTOR_IS_APPROX(sim.vnew, Eigen::VectorXd::Zero(model->nv), 1e-6); } BOOST_AUTO_TEST_CASE(simulator_instance_checkCollisionPairs) { ModelHandle model(new Model()); GeometryModelHandle geom_model(new GeometryModel()); // Add balls const double radius = 1.0; const double mass = 1.0; const std::string name1 = "ball_1"; const std::string name2 = "ball_1"; const std::string joint_name = "joint_1"; const std::string frame_name = "frame_1"; const Inertia inertia = Inertia::FromSphere(mass, radius); const JointModelFreeFlyer joint = JointModelFreeFlyer(); const JointIndex parent = 0; const SE3 placement = SE3::Identity(); JointIndex joint_id = model->addJoint(parent, joint, placement, joint_name); model->appendBodyToJoint(joint_id, inertia); hpp::fcl::CollisionGeometryPtr_t ball_ptr(new hpp::fcl::Sphere(radius)); const GeometryObject ball_geom1 = GeometryObject(name1, joint_id, placement, ball_ptr); const GeometryObject ball_geom2 = GeometryObject(name2, joint_id, placement, ball_ptr); GeomIndex ball_id1 = geom_model->addGeometryObject(ball_geom1); GeomIndex ball_id2 = geom_model->addGeometryObject(ball_geom2); CollisionPair col_pair(ball_id1, ball_id2); geom_model->addCollisionPair(col_pair); Simulator sim(model, geom_model); BOOST_CHECK(!sim.checkCollisionPairs()); } BOOST_AUTO_TEST_SUITE_END() ================================================ FILE: tests/forward/urdf-romeo.cpp ================================================ // // Copyright (c) 2024 INRIA // #include "simple/core/simulator.hpp" #include #include #include #include #include #include #include #include #include #include "tests/test_data/config.h" #include "../test-utils.hpp" using namespace simple; using namespace pinocchio; using ModelHandle = Simulator::ModelHandle; using DataHandle = Simulator::DataHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; using GeometryDataHandle = Simulator::GeometryDataHandle; using BilateralPointConstraintModel = Simulator::BilateralPointConstraintModel; using BilateralPointConstraintModelVector = Simulator::BilateralPointConstraintModelVector; using WeldConstraintModel = Simulator::WeldConstraintModel; using WeldConstraintModelVector = Simulator::WeldConstraintModelVector; #define ADMM ::pinocchio::ADMMContactSolverTpl using ModelHandle = Simulator::ModelHandle; using GeometryModelHandle = Simulator::GeometryModelHandle; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(simulator_instance_romeo_step) { const std::string urdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf"); std::vector package_dirs; const std::string mesh_dir = PINOCCHIO_MODEL_DIR; package_dirs.push_back(mesh_dir); Model _model; pinocchio::urdf::buildModel(urdf_filename, pinocchio::JointModelFreeFlyer(), _model); GeometryModel _geom_model; pinocchio::urdf::buildGeom(_model, urdf_filename, pinocchio::COLLISION, _geom_model, package_dirs); _geom_model.addAllCollisionPairs(); pinocchio::srdf::removeCollisionPairs(_model, _geom_model, srdf_filename, false); Data _data(_model); GeometryData _geom_data(_geom_model); pinocchio::srdf::loadReferenceConfigurations(_model, srdf_filename, false); Eigen::VectorXd q = _model.referenceConfigurations["half_sitting"]; Eigen::VectorXd v = Eigen::VectorXd::Zero(_model.nv); Eigen::VectorXd tau = Eigen::VectorXd::Zero(_model.nv); const double dt = 1e-3; std::shared_ptr model = std::make_shared(_model); std::shared_ptr data = std::make_shared(_data); std::shared_ptr geom_model = std::make_shared(_geom_model); std::shared_ptr geom_data = std::make_shared(_geom_data); Simulator sim(model, data, geom_model, geom_data); BOOST_CHECK_NO_THROW(sim.step(q, v, tau, dt)); BOOST_CHECK(sim.vfree.size() == model->nv); BOOST_CHECK(sim.vnew.size() == model->nv); BOOST_CHECK(sim.ftotal.size() == static_cast(model->njoints)); } BOOST_AUTO_TEST_SUITE_END() ================================================ FILE: tests/python/test_simulator_instance.py ================================================ import pinocchio as pin import simple import numpy as np from hppfcl import ( Halfspace, Sphere, HeightFieldAABB, CollisionRequest, CollisionResult, collide, ) try: import example_robot_data as erd has_erd = True except: print("Cannot find example robot data. Skipping simulator tests.") has_erd = False def setBallsAndHalfSpace(length=[0.2], mass=[1.0]): assert len(length) == len(mass) or len(length) == 1 or len(mass) == 1 N = max(len(length), len(mass)) if len(length) == 1: length = length * N if len(mass) == 1: mass = mass * N model = pin.Model() geom_model = pin.GeometryModel() # create plane for floor n = np.array([0.0, 0.0, 1]) plane_shape = Halfspace(n, 0) T = pin.SE3(np.eye(3), np.zeros(3)) plane = pin.GeometryObject("plane", 0, 0, T, plane_shape) plane.meshColor = np.array([0.5, 0.5, 0.5, 1.0]) plane_id = geom_model.addGeometryObject(plane) ball_ids = [] for n_ball in range(N): a = length[n_ball] m = mass[n_ball] freeflyer = pin.JointModelFreeFlyer() joint = model.addJoint(0, freeflyer, pin.SE3.Identity(), "ball_" + str(n_ball)) model.appendBodyToJoint( joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity() ) ball_shape = Sphere(a / 2) geom_ball = pin.GeometryObject( "ball_" + str(n_ball), joint, joint, pin.SE3.Identity(), ball_shape ) geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) ball_id = geom_model.addGeometryObject(geom_ball) for id in ball_ids: col_pair = pin.CollisionPair(id, ball_id) geom_model.addCollisionPair(col_pair) ball_ids += [ball_id] col_pair = pin.CollisionPair(plane_id, ball_id) geom_model.addCollisionPair(col_pair) model.qref = pin.neutral(model) model.qinit = model.qref.copy() for n_ball in range(N): model.qinit[7 * n_ball] += a model.qinit[7 * n_ball + 2] += 0.1 data = model.createData() geom_data = geom_model.createData() for req in geom_data.collisionRequests: req.security_margin = 1e-3 return model, data, geom_model, geom_data def test_init(): model = pin.Model() geom_model = pin.GeometryModel() data = model.createData() geom_data = geom_model.createData() sim = simple.Simulator(model, data, geom_model, geom_data) assert True def test_init2(): model = pin.Model() geom_model = pin.GeometryModel() sim = simple.Simulator(model, geom_model) assert True def test_simulator_handles(): model = pin.Model() geom_model = pin.GeometryModel() data = model.createData() geom_data = geom_model.createData() sim = simple.Simulator(model, data, geom_model, geom_data) model2 = pin.buildSampleModelHumanoid() model2.lowerPositionLimit = -np.ones((model2.nq, 1)) model2.upperPositionLimit = np.ones((model2.nq, 1)) geom_model2 = pin.buildSampleGeometryModelHumanoid(model2) data2 = model2.createData() geom_data2 = geom_model2.createData() sim = simple.Simulator(model2, data2, geom_model2, geom_data2) q = pin.randomConfiguration(model2) pin.updateGeometryPlacements(sim.model, sim.data, sim.geom_model, sim.geom_data, q) assert sim.model == model2 assert not (sim.model == model) assert sim.geom_model == geom_model2 assert not (sim.geom_model == geom_model) assert sim.geom_data == geom_data2 assert not (sim.geom_data == geom_data) model3 = model2.copy() freeflyer = pin.JointModelFreeFlyer() joint = model2.addJoint(0, freeflyer, pin.SE3.Identity(), "ball_0") assert sim.model.njoints == model3.njoints + 1 gemo_model3 = geom_model2.copy() ball_shape = Sphere(0.1 / 2) geom_ball = pin.GeometryObject( "ball_0", joint, joint, pin.SE3.Identity(), ball_shape ) ball_id = geom_model2.addGeometryObject(geom_ball) assert sim.geom_model.ngeoms == gemo_model3.ngeoms + 1 def test_init_empty(): model = pin.Model() geom_model = pin.GeometryModel() data = model.createData() geom_data = geom_model.createData() sim = simple.Simulator(model, data, geom_model, geom_data) q = pin.neutral(model) v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) assert True def test_step(): model = pin.buildSampleModelManipulator() geom_model = pin.buildSampleGeometryModelManipulator(model) data = model.createData() geom_data = geom_model.createData() sim = simple.Simulator(model, data, geom_model, geom_data) q = pin.randomConfiguration(model) v = np.random.random(model.nv) tau = np.random.random(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) fext = [pin.Force(np.random.random(6)) for i in range(model.njoints)] sim.step(q, v, tau, fext, dt) assert True def test_step_balls(): mass = [12.0] length = [1.5] model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass) q = pin.neutral(model) q[2] = length[0] / 2.0 + 1.0 v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim = simple.Simulator(model, data, geom_model, geom_data) sim.step(q, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 q[2] = 0.0 v = np.zeros(model.nv) sim.step(q, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 1 assert np.linalg.norm(sim.vnew) < 1e-6 mass = [12.0, 0.1] length = [1.5, 0.5] model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass) sim = simple.Simulator(model, data, geom_model, geom_data) q = pin.neutral(model) q[0] = 0.0 q[7] = length[1] / 2.0 + 1.0 q[2] = 0.0 q[9] = 0.0 v = np.zeros(model.nv) tau = np.zeros(model.nv) sim.step(q, v, tau, dt) assert np.linalg.norm(sim.vnew) < 1e-6 num_contacts = sim.constraints_problem.getNumberOfContacts() assert num_contacts == 2 lam = sim.constraints_problem.frictional_point_constraints_forces()[ : 3 * num_contacts ] assert len(lam) == 3 * num_contacts sim.step(q, v, tau, dt) assert True def test_step_balls_with_constraints(): nballs = 2 mass = [23.0] * nballs length = [2.0] * nballs model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass) model.lowerPositionLimit = np.ones(model.nq) * -100.0 model.upperPositionLimit = np.ones(model.nq) * 100.0 q0 = pin.neutral(model) for i in range(nballs): q0[7 * i] = i * length[i] * 1.5 q0[7 * i + 2] = 10.0 q = q0.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim = simple.Simulator(model, data, geom_model, geom_data) sim.step(q, v, tau, dt) vnew = v + dt * pin.aba(model, data, q, v, tau) assert sim.constraints_problem.constraints_problem_size() == nballs * 3 * 2 assert np.linalg.norm(sim.vnew - vnew) < 1e-6 model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass) model.lowerPositionLimit = np.ones(model.nq) * -100.0 model.upperPositionLimit = np.ones(model.nq) * 100.0 for i in range(nballs // 2): model.lowerPositionLimit[7 * i + 2] = 10.0 q = q0.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim = simple.Simulator(model, data, geom_model, geom_data) sim.step(q, v, tau, dt) vnew = v + dt * pin.aba(model, data, q, v, tau) assert sim.constraints_problem.constraints_problem_size() == nballs * 3 * 2 assert ( np.linalg.norm(sim.vnew[(nballs // 2) * 6 :] - vnew[(nballs // 2) * 6 :]) < 1e-6 ) assert np.linalg.norm(sim.vnew[: (nballs // 2) * 6]) < 1e-6 def addSystemCollisionPairs(model, geom_model, qref): """ Add the right collision pairs of a model, given qref. qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered to always be in collision. """ data = model.createData() geom_data = geom_model.createData() pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref) geom_model.removeAllCollisionPairs() num_col_pairs = 0 for i in range(len(geom_model.geometryObjects)): for j in range(i + 1, len(geom_model.geometryObjects)): # Don't add collision pair if same object if i != j: gobj_i: pin.GeometryObject = geom_model.geometryObjects[i] gobj_j: pin.GeometryObject = geom_model.geometryObjects[j] if gobj_i.name == "floor" or gobj_j.name == "floor": num_col_pairs += 1 col_pair = pin.CollisionPair(i, j) geom_model.addCollisionPair(col_pair) else: if gobj_i.parentJoint != gobj_j.parentJoint: # Compute collision between the geometries. Only add the collision pair if there is no collision. M1 = geom_data.oMg[i] M2 = geom_data.oMg[j] colreq = CollisionRequest() colreq.security_margin = 1e-2 # 1cm of clearance colres = CollisionResult() collide( gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres ) if not colres.isCollision(): num_col_pairs += 1 col_pair = pin.CollisionPair(i, j) geom_model.addCollisionPair(col_pair) def addFloor(geom_model: pin.GeometryModel): floor_collision_shape = Halfspace(0, 0, 1, 0) M = pin.SE3.Identity() floor_collision_object = pin.GeometryObject("floor", 0, 0, M, floor_collision_shape) geom_model.addGeometryObject(floor_collision_object) def test_manipulator_limits(): model = pin.buildSampleModelManipulator() geom_model = pin.buildSampleGeometryModelManipulator(model) q0 = pin.neutral(model) model.lowerPositionLimit = q0.copy() model.upperPositionLimit = q0.copy() sim = simple.Simulator(model, geom_model) v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q0, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq assert np.linalg.norm(sim.vnew) < 1e-6 def test_humanoid_limits(): model = pin.buildSampleModelHumanoid() geom_model = pin.buildSampleGeometryModelHumanoid(model) q0 = pin.neutral(model) model.lowerPositionLimit = q0.copy() model.upperPositionLimit = q0.copy() sim = simple.Simulator(model, geom_model) v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q0, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == 2 * (model.nq - 4) assert sim.admm_constraint_solver.getAbsoluteConvergenceResidual() < 1e-6 def test_freeflyer_limits(): model = pin.Model() geom_model = pin.GeometryModel() a = 0.1 m = 3.8 freeflyer = pin.JointModelFreeFlyer() joint = model.addJoint(0, freeflyer, pin.SE3.Identity(), "ball") model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity()) ball_shape = Sphere(a / 2) geom_ball = pin.GeometryObject("ball", joint, joint, pin.SE3.Identity(), ball_shape) geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) ball_id = geom_model.addGeometryObject(geom_ball) q0 = pin.neutral(model) model.lowerPositionLimit = q0.copy() model.upperPositionLimit = q0.copy() sim = simple.Simulator(model, geom_model) v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q0, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == 2 * (model.nq - 4) assert np.linalg.norm(sim.vnew) < 1e-6 def test_composite_limits(): model = pin.Model() geom_model = pin.GeometryModel() a = 0.1 m = 3.8 composite = pin.JointModelComposite(2) composite.addJoint(pin.JointModelRX()) composite.addJoint(pin.JointModelRY()) joint = model.addJoint(0, composite, pin.SE3.Identity(), "ball") model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity()) ball_shape = Sphere(a / 2) geom_ball = pin.GeometryObject("ball", joint, joint, pin.SE3.Identity(), ball_shape) geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) ball_id = geom_model.addGeometryObject(geom_ball) q0 = pin.neutral(model) model.lowerPositionLimit = q0.copy() model.upperPositionLimit = q0.copy() sim = simple.Simulator(model, geom_model) v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q0, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq assert np.linalg.norm(sim.vnew) < 1e-6 model = pin.Model() geom_model = pin.GeometryModel() a = 0.1 m = 3.8 composite = pin.JointModelComposite(2) composite.addJoint(pin.JointModelPX()) composite.addJoint(pin.JointModelPY()) joint = model.addJoint(0, composite, pin.SE3.Identity(), "ball") model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity()) ball_shape = Sphere(a / 2) geom_ball = pin.GeometryObject("ball", joint, joint, pin.SE3.Identity(), ball_shape) geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) ball_id = geom_model.addGeometryObject(geom_ball) q0 = pin.neutral(model) model.lowerPositionLimit = q0.copy() model.upperPositionLimit = q0.copy() sim = simple.Simulator(model, geom_model) v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q0, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq assert np.linalg.norm(sim.vnew) < 1e-6 def test_mujoco_humanoid_limits(): import os path = os.path.abspath(__file__) path = path.split("/") path = "/".join(path[:-2]) path = os.path.join(path, "test_data/mujoco_humanoid.xml") model = pin.buildModelFromMJCF(path) geom_model = pin.buildGeomFromMJCF(model, path, pin.COLLISION) addFloor(geom_model) q0 = pin.neutral(model) addSystemCollisionPairs(model, geom_model, q0) sim = simple.Simulator(model, geom_model) # First we test without contact but with limits q = q0.copy() q[2] += 1.0 v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) data = model.createData() vnew = v + dt * pin.aba(model, data, q, v, tau) assert np.linalg.norm(sim.vnew - vnew) < 1e-6 assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == (model.nq - 4) * 2 # Now we test without contact but with tight limits model.lowerPositionLimit = q0.copy() model.lowerPositionLimit[2] += 1.0 model.upperPositionLimit = model.lowerPositionLimit.copy() sim = simple.Simulator(model, geom_model) q = model.lowerPositionLimit.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 0 assert sim.constraints_problem.constraints_problem_size() == (model.nq - 4) * 2 assert sim.admm_constraint_solver.getAbsoluteConvergenceResidual() < 1e-6 # Now we test with contact but without limits model.lowerPositionLimit = -np.ones(model.nq) * np.finfo("d").max model.upperPositionLimit = np.ones(model.nq) * np.finfo("d").max sim = simple.Simulator(model, geom_model) q = q0.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) assert ( sim.constraints_problem.constraints_problem_size() == sim.constraints_problem.getNumberOfContacts() * 3 ) assert sim.admm_constraint_solver.getAbsoluteConvergenceResidual() < 1e-6 # Now we test with contact and with large limits model.lowerPositionLimit = -np.ones(model.nq) * 10000.0 model.upperPositionLimit = np.ones(model.nq) * 10000.0 sim = simple.Simulator(model, geom_model) q = q0.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) assert ( sim.constraints_problem.constraints_problem_size() == (model.nq - 4) * 2 + 3 * sim.constraints_problem.getNumberOfContacts() ) # TODO Now we test with contact and limits model = pin.buildModelFromMJCF(path) sim = simple.Simulator(model, geom_model) q = q0.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim.step(q, v, tau, dt) if has_erd: def setSolo(): # robot model robot = erd.load("solo12") model = robot.model.copy() model.qref = np.array( [ 0.09906518, 0.20099078, 0.32502457, 0.19414175, -0.00524735, -0.97855773, 0.06860185, 0.00968163, 0.60963582, -1.61206407, -0.02543309, 0.66709088, -1.50870083, 0.32405118, -1.15305599, 1.56867351, -0.39097222, -1.29675892, 1.39741073, ] ) model.qref = pin.normalize(model, model.qref) # Create height field ground def ground(xy): return ( np.sin(xy[0] * 3) / 5 + np.cos(xy[1] ** 2 * 3) / 20 + np.sin(xy[1] * xy[0] * 5) / 10 ) xg = np.arange(-2, 2, 0.02) nx = xg.shape[0] xy_g = np.meshgrid(xg, xg) xy_g = np.stack(xy_g) elev_g = np.zeros((nx, nx)) elev_g[:, :] = ground(xy_g) sx = xg[-1] - xg[0] sy = xg[-1] - xg[0] elev_g[:, :] = elev_g[::-1, :] # Create geometry model geom_model = robot.collision_model hfield = HeightFieldAABB(sx, sy, elev_g, np.min(elev_g)) Mhfield = pin.SE3.Identity() ground = pin.GeometryObject("ground", 0, Mhfield, hfield) ground_id = geom_model.addGeometryObject(ground) # Add collision pairs between the ground and the robot geom_model.removeAllCollisionPairs() a = 0.01910275 frames_names = ["HR_FOOT", "HL_FOOT", "FR_FOOT", "FL_FOOT"] for name in frames_names: frame_id = model.getFrameId(name) frame = model.frames[frame_id] joint_id = frame.parentJoint frame_placement = frame.placement shape_name = name + "_shape" shape = Sphere(a) geometry = pin.GeometryObject(shape_name, joint_id, frame_placement, shape) geometry.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) geom_id = geom_model.addGeometryObject(geometry) foot_plane = pin.CollisionPair( ground_id, geom_id ) # order should be inverted ? geom_model.addCollisionPair(foot_plane) # Create data data = model.createData() geom_data = geom_model.createData() for req in geom_data.collisionRequests: req.security_margin = 1e-3 return model, data, geom_model, geom_data def test_step_solo(): model, data, geom_model, geom_data = setSolo() q = model.qref.copy() v = np.zeros(model.nv) tau = np.zeros(model.nv) dt = 1e-3 sim = simple.Simulator(model, data, geom_model, geom_data) sim.step(q, v, tau, dt) assert sim.constraints_problem.getNumberOfContacts() == 4 # test_init() # test_init2() # test_simulator_handles() # test_init_empty() # test_step() # test_step_balls() # test_step_balls_with_constraints() # test_humanoid_limits() # test_freeflyer_limits() # test_composite_limits() # test_manipulator_limits() # test_mujoco_humanoid_limits() # print("All tests passed") ================================================ FILE: tests/test-utils.hpp ================================================ #ifndef __simple__test_utils_hpp__ #define __simple__test_utils_hpp__ #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE( \ ((Va) - (Vb)).isZero(precision), "check " #Va ".isApprox(" #Vb ") failed at precision " \ << precision << ". (" #Va " - " #Vb ").norm() = " << ((Va) - (Vb)).norm() << " [\n" \ << (Va).transpose() << "\n!=\n" \ << (Vb).transpose() << "\n]") #define INDEX_EQUALITY_CHECK(i1, i2) BOOST_CHECK_MESSAGE(i1 == i2, "check " #i1 "==" #i2 " failed. [" << i1 << " != " << i2 << "]") #define INDEX_INEQUALITY_CHECK(i1, i2) BOOST_CHECK_MESSAGE(i1 <= i2, "check " #i1 "==" #i2 " failed. [" << i1 << " != " << i2 << "]") #define REAL_IS_APPROX(a, b, precision) \ BOOST_CHECK_MESSAGE( \ std::abs((a) - (b)) < precision, \ "check std::abs(" #a " - " #b ") = " << std::abs((a) - (b)) << " < " << precision << " failed. [" << a << " != " << b << "]") #endif ================================================ FILE: tests/test_data/config.h.in ================================================ // // Copyright (c) 2024 INRIA // #ifndef __simple_test_data_config_h__ #define __simple_test_data_config_h__ #define SIMPLE_TEST_DATA_DIR "@PROJECT_SOURCE_DIR@/tests/test_data" #endif // __simple_test_data_config_h__ ================================================ FILE: tests/test_data/mujoco_humanoid.xml ================================================