Showing preview only (467K chars total). Download the full file or copy to clipboard to get everything.
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 <benchmark/benchmark.h>
#include <Eigen/Core>
#define N 100
#define WARMUP_TIME 1.
#define MIN_TIME 2.
using Vec3f = Eigen::Matrix<float, 3, 1>;
using Mat3f = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
using Vec4f = Eigen::Matrix<float, 4, 1>;
using Mat4f = Eigen::Matrix<float, 4, 4, Eigen::RowMajor>;
using Mat34f = Eigen::Matrix<float, 3, 4, Eigen::RowMajor>;
using Vec3d = Eigen::Matrix<double, 3, 1>;
using Mat3d = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;
using Vec4d = Eigen::Matrix<double, 4, 1>;
using Mat4d = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>;
using Mat34d = Eigen::Matrix<double, 3, 4, Eigen::RowMajor>;
using Vec3i16 = Eigen::Matrix<std::int16_t, 3, 1>;
using Mat3i16 = Eigen::Matrix<std::int16_t, 3, 3, Eigen::RowMajor>;
using Vec4i16 = Eigen::Matrix<std::int16_t, 4, 1>;
using Mat4i16 = Eigen::Matrix<std::int16_t, 4, 4, Eigen::RowMajor>;
using Mat34i16 = Eigen::Matrix<std::int16_t, 3, 4, Eigen::RowMajor>;
template<typename ResVec, typename InVec, typename Mat>
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<ResVec, Eigen::aligned_allocator<ResVec>> y;
std::vector<InVec, Eigen::aligned_allocator<InVec>> x;
std::vector<InVec, Eigen::aligned_allocator<InVec>> t;
std::vector<Mat, Eigen::aligned_allocator<Mat>> M;
std::vector<typename ResVec::Scalar> dotprod;
};
using Bench33f = BenchFixture<Vec3f, Vec3f, Mat3f>;
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<Vec3d, Vec3d, Mat3d>;
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<Vec4f, Vec4f, Mat4f>;
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<Vec4d, Vec4d, Mat4d>;
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<Vec3f, Vec4f, Mat34f>;
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<Vec3d, Vec4d, Mat34d>;
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<Vec4f, Vec4f, Mat34f>;
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<Vec4d, Vec4d, Mat34d>;
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<Vec3f, Vec3f, Mat3f>;
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<Vec3d, Vec3d, Mat3d>;
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<Vec4f, Vec4f, Mat4f>;
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<Vec4d, Vec4d, Mat4d>;
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<Vec4f, Vec4f, Mat4f>;
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<Vec4d, Vec4d, Mat4d>;
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 <pinocchio/algorithm/fwd.hpp>
#include <pinocchio/parsers/mjcf.hpp>
#include <benchmark/benchmark.h>
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<Model>();
GeometryModelHandle geom_model_handle = std::make_shared<GeometryModel>();
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<Simulator>(model_handle, geom_model_handle);
}
void TearDown(benchmark::State &)
{
}
std::unique_ptr<Simulator> 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<ADMM>(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<PGS>(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_<ConstraintsProblem::ContactMapper>(
"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>("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<ConstraintsProblem>;
bp::register_ptr_to_python<ConstraintsProblemHandle>();
ConstraintsProblemPythonVisitor<ConstraintsProblem>::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 <boost/python/tuple.hpp>
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<Simulator>::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 <eigenpy/eigenpy.hpp>
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<simple::context::Scalar, 6, 3, simple::context::Options>;
eigenpy::enableEigenPySpecific<Matrix6x3s>();
// 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 <pinocchio/bindings/python/utils/std-vector.hpp>
#include <pinocchio/bindings/python/utils/copyable.hpp>
#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<typename T>
// struct StdVectorOfReferenceWrappersPythonVisitor : public bp::def_visitor<StdVectorOfReferenceWrappersPythonVisitor<T>>
// {
// typedef std::vector<std::reference_wrapper<T>> Self;
//
// public:
// template<class PyClass>
// 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<bp::reference_existing_object>())
// .def("__len__", +[](const Self & self) -> std::size_t { return self.size(); });
// }
//
// static void expose(const std::string & name)
// {
// bp::class_<Self>(name.c_str(), "Vector of reference wrappers.", bp::no_init).def(StdVectorOfReferenceWrappersPythonVisitor());
// }
// };
template<typename ConstraintsProblem>
struct ConstraintsProblemPythonVisitor : public bp::def_visitor<ConstraintsProblemPythonVisitor<ConstraintsProblem>>
{
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<class PyClass>
void visit(PyClass & cl) const
{
cl
// ----------------------------------
// CONSTRUCTORS
.def(bp::init<ModelHandle, DataHandle, GeometryModelHandle, GeometryDataHandle>(
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<const ConstraintModel>::expose("StdRefVec_ConstraintModel");
// StdVectorOfReferenceWrappersPythonVisitor<ConstraintData>::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<typename ConstraintsProblem::BilateralPointConstraintModelVector, true>::expose(
"StdVec_BilateralPointConstraintModel");
::pinocchio::python::StdVectorPythonVisitor<typename ConstraintsProblem::WeldConstraintModelVector, true>::expose(
"StdVec_WeldConstraintModel");
::pinocchio::python::StdVectorPythonVisitor<std::vector<double>, true>::expose("StdVec_double");
::pinocchio::python::StdVectorPythonVisitor<std::vector<typename ConstraintsProblem::ContactMapper>, true>::expose(
"StdVec_ContactMapper");
::pinocchio::python::StdVectorPythonVisitor<std::vector<typename ConstraintsProblem::ContactMode>, true>::expose(
"StdVec_ContactMode");
bp::class_<typename ConstraintsProblem::WrappedConstraintModel>("WrappedConstraintModel", bp::no_init)
.def(
"__getattr__",
+[](bp::object self, std::string const & name) {
using ConstraintModel = typename ConstraintsProblem::ConstraintModel;
const ConstraintModel & obj = bp::extract<ConstraintModel>(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_<typename ConstraintsProblem::WrappedConstraintData>("WrappedConstraintData", bp::no_init)
.def(
"__getattr__",
+[](bp::object self, std::string const & name) {
using ConstraintData = typename ConstraintsProblem::ConstraintData;
const ConstraintData & obj = bp::extract<ConstraintData>(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<typename ConstraintsProblem::WrappedConstraintModelVector, true>::expose(
"StdVec_WrappedConstraintModel");
::pinocchio::python::StdVectorPythonVisitor<typename ConstraintsProblem::WrappedConstraintDataVector, true>::expose(
"StdVec_WrappedConstraintData");
bp::class_<ConstraintsProblem>("ConstraintsProblem", "Contact problem.\n", bp::no_init)
.def(ConstraintsProblemPythonVisitor<ConstraintsProblem>())
.def(::pinocchio::python::CopyableVisitor<ConstraintsProblem>());
}
};
} // 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 <pinocchio/bindings/python/utils/std-vector.hpp>
#include <pinocchio/bindings/python/utils/copyable.hpp>
// 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<typename Simulator>
struct SimulatorPythonVisitor : public boost::python::def_visitor<SimulatorPythonVisitor<Simulator>>
{
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<class PyClass>
void visit(PyClass & cl) const
{
cl
// -----------------------------
// CONSTRUCTORS
.def(bp::init<ModelHandle, DataHandle, GeometryModelHandle, GeometryDataHandle>(
bp::args("self", "model", "data", "geom_model", "geom_data"), "Constructor"))
.def(bp::init<ModelHandle, DataHandle, GeometryModelHandle, GeometryDataHandle, BilateralPointConstraintModelVector>(
bp::args("self", "model", "data", "geom_model", "geom_data", "bilateral_point_constraint_models"), "Constructor"))
.def(bp::init<ModelHandle, DataHandle, GeometryModelHandle, GeometryDataHandle, WeldConstraintModelVector>(
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<ModelHandle, GeometryModelHandle>(bp::args("self", "model", "geom_model"), "Constructor"))
.def(bp::init<ModelHandle, GeometryModelHandle, BilateralPointConstraintModelVector>(
bp::args("self", "model", "geom_model", "bilateral_point_constraint_models"), "Constructor"))
.def(bp::init<ModelHandle, GeometryModelHandle, WeldConstraintModelVector>(
bp::args("self", "model", "geom_model", "weld_constraint_models"), "Constructor"))
.def(bp::init<ModelHandle, GeometryModelHandle, BilateralPointConstraintModelVector, WeldConstraintModelVector>(
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<VectorXs> & 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<ModelHandle>();
const bp::converter::registration * reg = bp::converter::registry::query(info);
if (!reg)
{
bp::register_ptr_to_python<ModelHandle>();
}
}
{
const bp::type_info info = bp::type_id<DataHandle>();
const bp::converter::registration * reg = bp::converter::registry::query(info);
if (!reg)
{
bp::register_ptr_to_python<DataHandle>();
}
}
{
const bp::type_info info = bp::type_id<GeometryModelHandle>();
const bp::converter::registration * reg = bp::converter::registry::query(info);
if (!reg)
{
bp::register_ptr_to_python<GeometryModelHandle>();
}
}
{
const bp::type_info info = bp::type_id<GeometryDataHandle>();
const bp::converter::registration * reg = bp::converter::registry::query(info);
if (!reg)
{
bp::register_ptr_to_python<GeometryDataHandle>();
}
}
}
}
static void expose()
{
bp::class_<Simulator>("Simulator", "Instance of Simulator.", bp::no_init)
.def(SimulatorPythonVisitor<Simulator>())
.def(::pinocchio::python::CopyableVisitor<Simulator>());
{
bp::enum_<ConstraintSolverType>("ConstraintSolverType")
.value("PGS", ConstraintSolverType::PGS)
.value("ADMM", ConstraintSolverType::ADMM)
.value("NONE", ConstraintSolverType::NONE);
}
{
bp::class_<ConstraintSolverSettings>(
"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, bp::bases<ConstraintSolverSettings>>(
"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, bp::bases<ConstraintSolverSettings>>(
"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 <eigenpy/eigenpy.hpp>
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 <pinocchio/algorithm/delassus-operator-base.hpp>
#include <pinocchio/algorithm/contact-info.hpp>
#include <pinocchio/algorithm/constraints/coulomb-friction-cone.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/geometry.hpp>
namespace simple
{
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct traits<ConstraintsProblemTpl<_Scalar, _Options, JointCollectionTpl>>
{
using Scalar = _Scalar;
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct ConstraintsProblemTpl // : ::pinocchio::NumericalBase<ConstraintsProblemTpl<_Scalar, _Options,
// JointCollectionTpl>>
{
// 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<Scalar, Eigen::Dynamic, 1, Options>;
using MatrixXs = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>;
using Vector3s = Eigen::Matrix<Scalar, 3, 1, Options>;
using Vector6s = Eigen::Matrix<Scalar, 6, 1, Options>;
using Matrix3s = Eigen::Matrix<Scalar, 3, 3, Options>;
using MapVectorXs = Eigen::Map<VectorXs>;
using MapVector3s = Eigen::Map<Vector3s>;
using MapVector6s = Eigen::Map<Vector6s>;
using ContactIndex = std::size_t;
using Model = ::pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>;
using ModelHandle = std::shared_ptr<Model>;
using Data = typename Model::Data;
using DataHandle = std::shared_ptr<Data>;
using GeometryModelHandle = std::shared_ptr<::pinocchio::GeometryModel>;
using GeometryDataHandle = std::shared_ptr<::pinocchio::GeometryData>;
using ConstraintModel = ::pinocchio::ConstraintModelTpl<Scalar, Options>;
using ConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(ConstraintModel);
using WrappedConstraintModel = std::reference_wrapper<const ConstraintModel>;
using WrappedConstraintModelVector = std::vector<WrappedConstraintModel>;
using ConstraintData = ::pinocchio::ConstraintDataTpl<Scalar, Options>;
using ConstraintDataVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(ConstraintData);
using WrappedConstraintData = std::reference_wrapper<ConstraintData>;
using WrappedConstraintDataVector = std::vector<WrappedConstraintData>;
using FrictionalJointConstraintModel = ::pinocchio::FrictionalJointConstraintModelTpl<Scalar, Options>;
using FrictionalJointConstraintData = ::pinocchio::FrictionalJointConstraintDataTpl<Scalar, Options>;
using BilateralPointConstraintModel = ::pinocchio::BilateralPointConstraintModelTpl<Scalar, Options>;
using BilateralPointConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(BilateralPointConstraintModel);
using BilateralPointConstraintData = ::pinocchio::BilateralPointConstraintDataTpl<Scalar, Options>;
using WeldConstraintModel = ::pinocchio::WeldConstraintModelTpl<Scalar, Options>;
using WeldConstraintData = ::pinocchio::WeldConstraintDataTpl<Scalar, Options>;
using WeldConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(WeldConstraintModel);
using JointLimitConstraintModel = ::pinocchio::JointLimitConstraintModelTpl<Scalar, Options>;
using JointLimitConstraintData = ::pinocchio::JointLimitConstraintDataTpl<Scalar, Options>;
using BoxSet = ::pinocchio::BoxSetTpl<Scalar>;
using FrictionalPointConstraintModel = ::pinocchio::FrictionalPointConstraintModelTpl<Scalar, Options>;
using FrictionalPointConstraintData = ::pinocchio::FrictionalPointConstraintDataTpl<Scalar, Options>;
using CoulombFrictionCone = ::pinocchio::CoulombFrictionConeTpl<Scalar>;
using CoulombFrictionConeVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(CoulombFrictionCone);
using ConstraintCholeskyDecomposition = ::pinocchio::ContactCholeskyDecompositionTpl<Scalar, Options>;
using ContactPointVector = PINOCCHIO_ALIGNED_STD_VECTOR(Vector3s);
using ContactNormalVector = PINOCCHIO_ALIGNED_STD_VECTOR(Vector3s);
using SE3 = ::pinocchio::SE3Tpl<Scalar, Options>;
using PlacementVector = PINOCCHIO_ALIGNED_STD_VECTOR(SE3);
// operator to compute contact frames
using PlacementFromNormalAndPosition = PlacementFromNormalAndPositionTpl<Scalar, Options>;
/////////////////////////////////////////////////
/// 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<VectorXs, 2> 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<std::size_t> pairs_in_collision;
/// \brief The collision pairs that were in contact at the previous time step (before update function is called).
std::vector<bool> 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<std::size_t> 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<ContactMapper> 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<ContactMode> contact_modes;
/// \brief indexes of breaking contacts
std::vector<ContactIndex> breaking_contacts;
/// \brief indexes of sticking contacts
std::vector<ContactIndex> sticking_contacts;
/// \brief indexes of sliding contacts
std::vector<ContactIndex> 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<PlacementVector, 2> 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<std::size_t> 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<typename FreeVelocityVectorType, typename VelocityVectorType, typename VelocityWarmStartVectorType>
void build(
const Eigen::MatrixBase<FreeVelocityVectorType> & vfree,
const Eigen::MatrixBase<VelocityVectorType> & v,
const Eigen::MatrixBase<VelocityWarmStartVectorType> & 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<VectorXs> 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<const VectorXs> g() const
{
return this->m_g.head(this->constraints_problem_size());
}
/// \brief Getter for the preconditioner
Eigen::VectorBlock<VectorXs> preconditioner()
{
return this->m_preconditioner.head(this->constraints_problem_size());
}
/// \brief Const getter for the preconditioner
Eigen::VectorBlock<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<typename FreeVelocityVectorType, typename VelocityVectorType>
void computeConstraintsDrift(
const Eigen::MatrixBase<FreeVelocityVectorType> & vfree, const Eigen::MatrixBase<VelocityVectorType> & 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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<VectorXs> 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<const VectorXs> 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 <pinocchio/algorithm/contact-cholesky.hpp>
#include <pinocchio/algorithm/contact-info.hpp>
#include <pinocchio/algorithm/contact-jacobian.hpp>
#include <pinocchio/algorithm/contact-solver-utils.hpp>
#include <pinocchio/alloca.hpp>
#include <hpp/fcl/collision_data.h>
#include <boost/variant.hpp>
namespace simple
{
// --------------------------------------------------------------------------
template<typename S, int O, template<typename, int> class JointCollectionTpl>
ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::clear()
{
// Clearing joint limits
this->m_previous_joint_limits_constraint_forces_size = this->joint_limit_constraint_size();
JointLimitConstraintModel * jlcm = boost::get<JointLimitConstraintModel>(&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<typename S, int O, template<typename, int> class JointCollectionTpl>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<JointIndex> active_friction_joints_ids;
std::vector<int> 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<JointIndex> 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<Scalar>(friction_coefficients.getFrictionFromMaterialPair(material1.materialType, material2.materialType));
const Scalar compliance = static_cast<Scalar>(Scalar(0.5) * (material1.compliance + material2.compliance));
// const Scalar elasticity = static_cast<Scalar>(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<typename S, int O, template<typename, int> class JointCollectionTpl>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<JointLimitConstraintModel>(&constraint_model);
assert(jlcm != nullptr);
if (jlcm != nullptr)
{
JointLimitConstraintData * jlcd = boost::get<JointLimitConstraintData>(&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<std::size_t> & prev_active_set = this->m_previous_joint_limit_active_set;
assert(prev_active_set.empty() == false);
const std::vector<std::size_t> & 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<FrictionalPointConstraintModel>(&(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<const FrictionalPointConstraintData>(&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<Scalar>::dummy_precision()));
#endif // NDEBUG
// Warm-start the contact forces
const auto cindex = static_cast<Eigen::Index>(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<Scalar>::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<Eigen::Index>(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<typename S, int O, template<typename, int> class JointCollectionTpl>
template<typename FreeVelocityVectorType, typename VelocityVectorType, typename VelocityWarmStartVectorType>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::build(
const Eigen::MatrixBase<FreeVelocityVectorType> & vfree,
const Eigen::MatrixBase<VelocityVectorType> & v,
const Eigen::MatrixBase<VelocityWarmStartVectorType> & 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<typename S, int O, template<typename, int> class JointCollectionTpl>
template<typename FreeVelocityVectorType, typename VelocityVectorType>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::computeConstraintsDrift(
const Eigen::MatrixBase<FreeVelocityVectorType> & vfree, const Eigen::MatrixBase<VelocityVectorType> & 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<VectorXs>;
auto drift_visitor = ::simple::visitors::make_lambda_visitor(
//
// Visitor for FrictionalJointConstraintModel - velocity formulation
[&](const FrictionalJointConstraintModel & cmodel) {
const FrictionalJointConstraintData & cdata_ = boost::get<const FrictionalJointConstraintData>(cdata);
cmodel.jacobianMatrixProduct(this->model(), this->data(), cdata_, vfree, gc);
},
//
// Visitor for BilateralConstraintModel - velocity formulation
[&](const BilateralPointConstraintModel & cmodel) {
const BilateralPointConstraintData & cdata_ = boost::get<const BilateralPointConstraintData>(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<const WeldConstraintData>(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<const JointLimitConstraintData>(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<const FrictionalPointConstraintData>(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<typename S, int O, template<typename, int> class JointCollectionTpl>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::updateConstraintsHolders()
{
this->m_current_constraints_pb_id = 1 - this->m_current_constraints_pb_id;
}
// --------------------------------------------------------------------------
template<typename S, int O, template<typename, int> class JointCollectionTpl>
bool ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
void ConstraintsProblemTpl<S, O, JointCollectionTpl>::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<Eigen::Index>(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<typename S, int O, template<typename, int> class JointCollectionTpl>
bool ConstraintsProblemTpl<S, O, JointCollectionTpl>::check() const
{
const auto problem_size = static_cast<Eigen::Index>(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<bool>(
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<context::Scalar, context::Options, ::pinocchio::JointCollectionDefaultTpl>;
} // 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 <pinocchio/spatial/se3.hpp>
#include <pinocchio/multibody/geometry-object.hpp>
#ifndef SIMPLE_SKIP_COLLISION_DERIVATIVES_CONTRIBUTIONS
#include <diffcoal/contact_patch_derivative.hpp>
#endif
namespace simple
{
/// @brief Operator to construct a placement from a given normal and position.
template<typename _Scalar, int _Options>
struct PlacementFromNormalAndPositionTpl
{
using Scalar = _Scalar;
enum
{
Options = _Options
};
using SE3 = ::pinocchio::SE3Tpl<Scalar, Options>;
using Matrix6x3s = Eigen::Matrix<Scalar, 6, 3, Options>;
using Vector3s = Eigen::Matrix<Scalar, 3, 1, Options>;
/// \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<typename Vector3Normal, typename Vector3Position>
static void calc(
const Eigen::MatrixBase<Vector3Normal> & normal, // [input]
const Eigen::MatrixBase<Vector3Position> & 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<typename Matrix6x3Type1, typename Matrix6x3Type2>
static void calcDiff(
const SE3 & M, // [input]
const Eigen::MatrixBase<Matrix6x3Type1> & dM_dnormal, // [output]
const Eigen::MatrixBase<Matrix6x3Type2> & 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<typename Vector3>
static Vector3s getReferenceVector(const Eigen::MatrixBase<Vector3> & 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<Pointer &>(Base::geom1) = go1_ptr->geometry.get();
const_cast<Pointer &>(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<typename S, int O>
template<typename Vector3Normal, typename Vector3Position>
void PlacementFromNormalAndPositionTpl<S, O>::calc(
const Eigen::MatrixBase<Vector3Normal> & normal, // [input]
const Eigen::MatrixBase<Vector3Position> & 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<typename S, int O>
template<typename Matrix6x3Type1, typename Matrix6x3Type2>
void PlacementFromNormalAndPositionTpl<S, O>::calcDiff(
const SE3 & M, // [input]
const Eigen::MatrixBase<Matrix6x3Type1> & dM_dnormal_, // [output]
const Eigen::MatrixBase<Matrix6x3Type2> & 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<Matrix6x3Type1 &>(dM_dnormal_.derived());
Matrix6x3Type2 & dM_dposition = const_cast<Matrix6x3Type2 &>(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<Matrix6x3Type1, 3, 3>;
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 <pinocchio/utils/reference.hpp>
namespace simple
{
namespace helper
{
using pinocchio::helper::get_pointer;
using pinocchio::helper::get_ref;
} // namespace helper
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>
struct ConstraintsProblemTpl;
typedef ConstraintsProblemTpl<context::Scalar, context::Options> ConstraintsProblem;
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>
struct SimulatorTpl;
typedef SimulatorTpl<context::Scalar, context::Options> Simulator;
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>
struct ContactSolverDerivativesTpl;
typedef ContactSolverDerivativesTpl<context::Scalar, context::Options> ContactSolverDerivatives;
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl = ::pinocchio::JointCollectionDefaultTpl>
struct SimulatorDerivativesTpl;
typedef SimulatorDerivativesTpl<context::Scalar, context::Options> SimulatorDerivatives;
template<typename Scalar, int Options>
struct PlacementFromNormalAndPositionTpl;
typedef PlacementFromNormalAndPositionTpl<context::Scalar, context::Options> 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 <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/algorithm/admm-solver.hpp>
#include <pinocchio/algorithm/pgs-solver.hpp>
#include <pinocchio/algorithm/proximal.hpp>
#include <pinocchio/collision/broadphase-manager.hpp>
#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
namespace simple
{
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct traits<SimulatorTpl<_Scalar, _Options, JointCollectionTpl>>
{
using Scalar = _Scalar;
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct SimulatorTpl // : ::pinocchio::NumericalBase<SimulatorTpl<_Scalar,
// _Options, JointCollectionTpl>>
{
// TODO: template by allocator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Typedefs
using Scalar = _Scalar;
enum
{
Options = _Options
};
using GeomIndex = pinocchio::GeomIndex;
using VectorXs = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>;
using Vector3s = Eigen::Matrix<Scalar, 3, 1, Options>;
using Vector6s = Eigen::Matrix<Scalar, 6, 1, Options>;
using MatrixXs = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>;
using Model = ::pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>;
using ModelHandle = std::shared_ptr<Model>;
using Data = typename Model::Data;
using DataHandle = std::shared_ptr<Data>;
using GeometryModel = ::pinocchio::GeometryModel;
using GeometryModelHandle = std::shared_ptr<GeometryModel>;
using GeometryData = ::pinocchio::GeometryData;
using GeometryDataHandle = std::shared_ptr<GeometryData>;
using Force = typename Data::Force;
using Motion = typename Data::Motion;
using SE3 = typename Data::SE3;
using JointModel = ::pinocchio::JointModelTpl<Scalar, Options, JointCollectionTpl>;
// TODO: template simulator by broad phase manager
using BroadPhaseManager = ::pinocchio::BroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager>;
using BroadPhaseManagerHandle = std::shared_ptr<BroadPhaseManager>;
using CollisionCallbackCollect = ::pinocchio::CollisionCallBackCollect;
using ADMMConstraintSolver = ::pinocchio::ADMMContactSolverTpl<Scalar>;
using PGSConstraintSolver = ::pinocchio::PGSContactSolverTpl<Scalar>;
using SpatialForce = ::pinocchio::ForceTpl<Scalar, Options>;
using SpatialForceVector = PINOCCHIO_ALIGNED_STD_VECTOR(SpatialForce);
using ConstraintsProblem = ConstraintsProblemTpl<Scalar, Options, JointCollectionTpl>;
using ConstraintsProblemHandle = std::shared_ptr<ConstraintsProblem>;
using ConstraintCholeskyDecomposition = typename ConstraintsProblem::ConstraintCholeskyDecomposition;
using DelassusOperator = ::pinocchio::DelassusCholeskyExpressionTpl<ConstraintCholeskyDecomposition>;
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<typename> class ConstraintSolver = ::pinocchio::ADMMContactSolverTpl,
typename ConfigVectorType,
typename VelocityVectorType,
typename TorqueVectorType>
void step(
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<VelocityVectorType> & v,
const Eigen::MatrixBase<TorqueVectorType> & 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<typename> class ConstraintSolver = ::pinocchio::ADMMContactSolverTpl,
typename ConfigVectorType,
typename VelocityVectorType,
typename TorqueVectorType,
typename ForceDerived>
void step(
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<VelocityVectorType> & v,
const Eigen::MatrixBase<TorqueVectorType> & tau,
const pinocchio::container::aligned_vector<ForceDerived> & 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<template<typename> class ConstraintSolver, typename VelocityVectorType>
void resolveConstraints(const Eigen::MatrixBase<VelocityVectorType> & v, const Scalar dt);
};
namespace details
{
template<template<typename> class SolverTpl, typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct SimulatorConstraintSolverTpl
{
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl>
{
using Scalar = _Scalar;
enum
{
Options = _Options
};
using Simulator = SimulatorTpl<Scalar, Options, JointCollectionTpl>;
using ConstraintsProblem = typename Simulator::ConstraintsProblem;
using ADMMConstraintSolverSettings = typename Simulator::ADMMConstraintSolverSettings;
using ADMMConstraintSolver = ::pinocchio::ADMMContactSolverTpl<Scalar>;
using MatrixXs = typename Simulator::MatrixXs;
using VectorXs = typename Simulator::VectorXs;
using RefConstVectorXs = Eigen::Ref<const VectorXs>;
static void run(Simulator & simulator, Scalar dt);
protected:
static void setup(Simulator & simulator);
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl>
{
using Scalar = _Scalar;
enum
{
Options = _Options
};
using Simulator = SimulatorTpl<Scalar, Options, JointCollectionTpl>;
using ConstraintsProblem = typename Simulator::ConstraintsProblem;
using PGSConstraintSolverSettings = typename Simulator::PGSConstraintSolverSettings;
using PGSConstraintSolver = ::pinocchio::PGSContactSolverTpl<Scalar>;
using DelassusOperatorDense = ::pinocchio::DelassusOperatorDenseTpl<Scalar>;
using MatrixXs = typename Simulator::MatrixXs;
using VectorXs = typename Simulator::VectorXs;
using RefConstVectorXs = Eigen::Ref<const VectorXs>;
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 <pinocchio/algorithm/contact-cholesky.hpp>
#include <pinocchio/algorithm/contact-info.hpp>
#include <pinocchio/algorithm/contact-jacobian.hpp>
#include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/algorithm/contact-inverse-dynamics.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/collision/collision.hpp>
#include <pinocchio/collision/broadphase.hpp>
#include "simple/tracy.hpp"
// #include <pinocchio/algorithm/contact-jacobian.hpp>
#include <pinocchio/multibody/fwd.hpp>
#include <hpp/fcl/collision_data.h>
#include <boost/variant.hpp>
namespace simple
{
// --------------------------------------------------------------------------
template<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::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<std::size_t>(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<std::size_t>(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<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::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<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::SimulatorTpl(
ModelHandle model_handle,
GeometryModelHandle geom_model_handle,
const BilateralPointConstraintModelVector & bilateral_constraint_models)
: SimulatorTpl(model_handle, geom_model_handle, bilateral_constraint_models, WeldConstraintModelVector())
{
}
// --------------------------------------------------------------------------
template<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::SimulatorTpl(
ModelHandle model_handle, GeometryModelHandle geom_model_handle, const WeldConstraintModelVector & weld_constraint_models)
: SimulatorTpl(model_handle, geom_model_handle, BilateralPointConstraintModelVector(), weld_constraint_models)
{
}
// --------------------------------------------------------------------------
template<typename S, int O, template<typename, int> class JointCollectionTpl>
SimulatorTpl<S, O, JointCollectionTpl>::SimulatorTpl(ModelHandle model_handle, GeometryModelHandle geom_model_handle)
: SimulatorTpl(model_handle, geom_model_handle, BilateralPointConstraintModelVector(), WeldConstraintModelVector())
{
}
// --------------------------------------------------------------------------
template<typename S, int O, template<typename, int> class JointCollectionTpl>
void SimulatorTpl<S, O, JointCollectionTpl>::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<std::size_t>(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<std::size_t>(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);
}
// ------------------
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
SYMBOL INDEX (292 symbols across 39 files)
FILE: benchmark/affine-transform.cpp
type BenchFixture (line 27) | struct BenchFixture : benchmark::Fixture
method SetUp (line 29) | void SetUp(benchmark::State &)
method TearDown (line 46) | void TearDown(benchmark::State &)
FILE: benchmark/mujoco-humanoid.cpp
function addFloorToGeomModel (line 25) | void addFloorToGeomModel(GeometryModel & geom_model)
function addSystemCollisionPairs (line 36) | void addSystemCollisionPairs(const Model & model, GeometryModel & geom_m...
type MujocoHumanoidFixture (line 84) | struct MujocoHumanoidFixture : benchmark::Fixture
method SetUp (line 86) | void SetUp(benchmark::State &)
method TearDown (line 109) | void TearDown(benchmark::State &)
FILE: bindings/python/core/expose-constraints-problem.cpp
type simple (line 9) | namespace simple
type python (line 11) | namespace python
function exposeConstraintsProblem (line 14) | void exposeConstraintsProblem()
FILE: bindings/python/core/expose-contact-frame.cpp
type simple (line 12) | namespace simple
type python (line 14) | namespace python
function SE3 (line 20) | SE3 placementFromNormalAndPosition(const Vector3s & normal, const Ve...
function placementFromNormalAndPositionDerivative (line 27) | bp::tuple placementFromNormalAndPositionDerivative(const SE3 & M)
function exposeContactFrame (line 35) | void exposeContactFrame()
FILE: bindings/python/core/expose-simulator.cpp
type simple (line 7) | namespace simple
type python (line 9) | namespace python
function exposeSimulator (line 12) | void exposeSimulator()
FILE: bindings/python/module.cpp
function BOOST_PYTHON_MODULE (line 14) | BOOST_PYTHON_MODULE(SIMPLE_PYTHON_MODULE_NAME)
FILE: include/simple/bindings/python/core/constraints-problem.hpp
type simple (line 20) | namespace simple
type python (line 22) | namespace python
type ConstraintsProblemPythonVisitor (line 48) | struct ConstraintsProblemPythonVisitor : public bp::def_visitor<Cons...
method visit (line 65) | void visit(PyClass & cl) const
method expose (line 211) | static void expose()
FILE: include/simple/bindings/python/core/simulator.hpp
type simple (line 26) | namespace simple
type python (line 28) | namespace python
type SimulatorPythonVisitor (line 33) | struct SimulatorPythonVisitor : public boost::python::def_visitor<Si...
method visit (line 59) | void visit(PyClass & cl) const
method expose (line 287) | static void expose()
FILE: include/simple/bindings/python/fwd.hpp
type simple (line 10) | namespace simple
type python (line 12) | namespace python
FILE: include/simple/core/constraints-problem.hpp
type simple (line 18) | namespace simple
type traits<ConstraintsProblemTpl<_Scalar, _Options, JointCollectionTpl>> (line 22) | struct traits<ConstraintsProblemTpl<_Scalar, _Options, JointCollection...
type ConstraintsProblemTpl (line 28) | struct ConstraintsProblemTpl // : ::pinocchio::NumericalBase<Constrain...
type ContactMapper (line 218) | struct ContactMapper
method ContactMapper (line 220) | ContactMapper(std::size_t begin, std::size_t count)
type ContactMode (line 241) | enum struct ContactMode
method Model (line 322) | const Model & model() const
method Model (line 329) | Model & model()
method Data (line 336) | const Data & data() const
method Data (line 343) | Data & data()
method constraints_problem_size (line 407) | int constraints_problem_size() const
method previous_constraints_problem_size (line 417) | int previous_constraints_problem_size() const
method g (line 427) | Eigen::VectorBlock<VectorXs> g()
method g (line 433) | Eigen::VectorBlock<const VectorXs> g() const
method preconditioner (line 439) | Eigen::VectorBlock<VectorXs> preconditioner()
method preconditioner (line 445) | Eigen::VectorBlock<const VectorXs> preconditioner() const
method time_scaling_acc_to_constraints (line 451) | Eigen::VectorBlock<VectorXs> time_scaling_acc_to_constraints()
method time_scaling_acc_to_constraints (line 457) | Eigen::VectorBlock<const VectorXs> time_scaling_acc_to_constraints()...
method constraints_forces (line 463) | Eigen::VectorBlock<VectorXs> constraints_forces()
method constraints_forces (line 469) | Eigen::VectorBlock<const VectorXs> constraints_forces() const
method previous_constraints_forces (line 475) | Eigen::VectorBlock<VectorXs> previous_constraints_forces()
method previous_constraints_forces (line 481) | Eigen::VectorBlock<const VectorXs> previous_constraints_forces() const
method constraints_velocities (line 488) | Eigen::VectorBlock<VectorXs> constraints_velocities()
method constraints_velocities (line 495) | Eigen::VectorBlock<const VectorXs> constraints_velocities() const
method constraints_velocities_warmstarts (line 501) | Eigen::VectorBlock<VectorXs> constraints_velocities_warmstarts()
method constraints_velocities_warmstarts (line 507) | Eigen::VectorBlock<const VectorXs> constraints_velocities_warmstarts...
method joint_friction_constraint_size (line 531) | int joint_friction_constraint_size() const
method getNumberOfJointFrictionConstraints (line 537) | std::size_t getNumberOfJointFrictionConstraints() const
method joint_friction_constraint_forces (line 547) | Eigen::VectorBlock<VectorXs> joint_friction_constraint_forces()
method joint_friction_constraint_forces (line 553) | Eigen::VectorBlock<const VectorXs> joint_friction_constraint_forces(...
method previous_joint_friction_constraint_forces (line 559) | Eigen::VectorBlock<VectorXs> previous_joint_friction_constraint_forc...
method previous_joint_friction_constraint_forces (line 565) | Eigen::VectorBlock<const VectorXs> previous_joint_friction_constrain...
method bilateral_constraints_size (line 574) | int bilateral_constraints_size() const
method getNumberOfBilateralConstraints (line 580) | std::size_t getNumberOfBilateralConstraints() const
method bilateral_constraints_forces (line 586) | Eigen::VectorBlock<VectorXs> bilateral_constraints_forces()
method bilateral_constraints_forces (line 593) | Eigen::VectorBlock<const VectorXs> bilateral_constraints_forces() const
method previous_bilateral_constraints_forces (line 600) | Eigen::VectorBlock<VectorXs> previous_bilateral_constraints_forces()
method previous_bilateral_constraints_forces (line 607) | Eigen::VectorBlock<const VectorXs> previous_bilateral_constraints_fo...
method weld_constraints_size (line 617) | int weld_constraints_size() const
method getNumberOfWeldConstraints (line 623) | std::size_t getNumberOfWeldConstraints() const
method weld_constraints_forces (line 629) | Eigen::VectorBlock<VectorXs> weld_constraints_forces()
method weld_constraints_forces (line 636) | Eigen::VectorBlock<const VectorXs> weld_constraints_forces() const
method previous_weld_constraints_forces (line 643) | Eigen::VectorBlock<VectorXs> previous_weld_constraints_forces()
method previous_weld_constraints_forces (line 650) | Eigen::VectorBlock<const VectorXs> previous_weld_constraints_forces(...
method joint_limit_constraint_max_size (line 660) | int joint_limit_constraint_max_size() const
method joint_limit_constraint_size (line 669) | int joint_limit_constraint_size() const
method previous_joint_limit_constraint_size (line 675) | int previous_joint_limit_constraint_size() const
method getNumberOfJointLimitConstraints (line 681) | std::size_t getNumberOfJointLimitConstraints() const
method joint_limit_constraint_forces (line 691) | Eigen::VectorBlock<VectorXs> joint_limit_constraint_forces()
method joint_limit_constraint_forces (line 699) | Eigen::VectorBlock<const VectorXs> joint_limit_constraint_forces() c...
method previous_joint_limit_constraint_forces (line 707) | Eigen::VectorBlock<VectorXs> previous_joint_limit_constraint_forces()
method previous_joint_limit_constraint_forces (line 715) | Eigen::VectorBlock<const VectorXs> previous_joint_limit_constraint_f...
method frictional_point_constraints_size (line 728) | int frictional_point_constraints_size() const
method previous_frictional_point_constraints_size (line 734) | int previous_frictional_point_constraints_size() const
method setMaxNumberOfContactsPerCollisionPair (line 740) | void setMaxNumberOfContactsPerCollisionPair(ContactIndex num)
method ContactIndex (line 751) | ContactIndex getMaxNumberOfContactsPerCollisionPair() const
method ContactIndex (line 760) | ContactIndex getMaxNumberOfContacts() const
method ContactIndex (line 766) | ContactIndex getNumberOfContacts() const
method ContactIndex (line 772) | ContactIndex getPreviousNumberOfContacts() const
method frictional_point_constraints_forces (line 778) | Eigen::VectorBlock<VectorXs> frictional_point_constraints_forces()
method frictional_point_constraints_forces (line 789) | Eigen::VectorBlock<const VectorXs> frictional_point_constraints_forc...
method previous_frictional_point_constraints_forces (line 800) | Eigen::VectorBlock<VectorXs> previous_frictional_point_constraints_f...
method previous_frictional_point_constraints_forces (line 811) | Eigen::VectorBlock<const VectorXs> previous_frictional_point_constra...
method frictional_point_constraints_velocities (line 823) | Eigen::VectorBlock<VectorXs> frictional_point_constraints_velocities()
method frictional_point_constraints_velocities (line 835) | Eigen::VectorBlock<const VectorXs> frictional_point_constraints_velo...
method frictional_point_constraints_warmstart (line 846) | Eigen::VectorBlock<VectorXs> frictional_point_constraints_warmstart()
method frictional_point_constraints_warmstart (line 857) | Eigen::VectorBlock<const VectorXs> frictional_point_constraints_warm...
method contact_time_scaling_acc_to_constraints (line 868) | Eigen::VectorBlock<VectorXs> contact_time_scaling_acc_to_constraints()
method contact_time_scaling_acc_to_constraints (line 879) | Eigen::VectorBlock<const VectorXs> contact_time_scaling_acc_to_const...
method PlacementVector (line 890) | PlacementVector & frictional_point_constraints_placements()
method PlacementVector (line 896) | const PlacementVector & frictional_point_constraints_placements() const
method PlacementVector (line 902) | PlacementVector & previous_frictional_point_constraints_placements()
method PlacementVector (line 908) | const PlacementVector & previous_frictional_point_constraints_placem...
FILE: include/simple/core/constraints-problem.hxx
type simple (line 21) | namespace simple
FILE: include/simple/core/contact-frame.hpp
type simple (line 17) | namespace simple
type PlacementFromNormalAndPositionTpl (line 22) | struct PlacementFromNormalAndPositionTpl
method Vector3s (line 60) | static Vector3s getReferenceVector(const Eigen::MatrixBase<Vector3> ...
method Vector3s (line 76) | static Vector3s reference_vector()
method Vector3s (line 82) | static Vector3s other_reference_vector()
type ComputeContactPatchDerivative (line 90) | struct ComputeContactPatchDerivative : ::diffcoal::ComputeContactPatch...
method ComputeContactPatchDerivative (line 95) | ComputeContactPatchDerivative(const GeometryObject & go1, const Geom...
method run (line 104) | virtual void run(
method GeometryObject (line 128) | const GeometryObject & getGeometryObject1() const
method GeometryObject (line 132) | const GeometryObject & getGeometryObject2() const
FILE: include/simple/core/contact-frame.hxx
type simple (line 8) | namespace simple
FILE: include/simple/core/fwd.hpp
type simple (line 11) | namespace simple
type helper (line 13) | namespace helper
type ConstraintsProblemTpl (line 20) | struct ConstraintsProblemTpl
type SimulatorTpl (line 24) | struct SimulatorTpl
type ContactSolverDerivativesTpl (line 28) | struct ContactSolverDerivativesTpl
type SimulatorDerivativesTpl (line 32) | struct SimulatorDerivativesTpl
type PlacementFromNormalAndPositionTpl (line 36) | struct PlacementFromNormalAndPositionTpl
FILE: include/simple/core/simulator.hpp
type simple (line 22) | namespace simple
type traits<SimulatorTpl<_Scalar, _Options, JointCollectionTpl>> (line 26) | struct traits<SimulatorTpl<_Scalar, _Options, JointCollectionTpl>>
type SimulatorTpl (line 32) | struct SimulatorTpl // : ::pinocchio::NumericalBase<SimulatorTpl<_Scalar,
type ConstraintSolverSettings (line 151) | struct ConstraintSolverSettings
type ADMMConstraintSolverSettings (line 172) | struct ADMMConstraintSolverSettings : ConstraintSolverSettings
type PGSConstraintSolverSettings (line 189) | struct PGSConstraintSolverSettings : ConstraintSolverSettings
type ConstraintSolverType (line 200) | enum struct ConstraintSolverType
method getStepCPUTimes (line 224) | hpp::fcl::CPUTimes getStepCPUTimes() const
method getConstraintSolverCPUTimes (line 231) | hpp::fcl::CPUTimes getConstraintSolverCPUTimes() const
method getCollisionDetectionCPUTimes (line 237) | hpp::fcl::CPUTimes getCollisionDetectionCPUTimes() const
method isReset (line 384) | bool isReset() const
method Model (line 401) | const Model & model() const
method Model (line 408) | Model & model()
method Data (line 415) | const Data & data() const
method Data (line 422) | Data & data()
method ConstraintsProblem (line 457) | const ConstraintsProblem & constraints_problem() const
method ConstraintsProblem (line 464) | ConstraintsProblem & constraints_problem()
method ConstraintsProblemHandle (line 471) | ConstraintsProblemHandle getConstraintsProblemHandle() const
method preambleResolveConstraints (line 495) | virtual void preambleResolveConstraints(const Scalar dt)
type details (line 506) | namespace details
type SimulatorConstraintSolverTpl (line 509) | struct SimulatorConstraintSolverTpl
type SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTpl, _Scalar, _Options, JointCollectionTpl> (line 514) | struct SimulatorConstraintSolverTpl<::pinocchio::ADMMContactSolverTp...
type SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl, _Scalar, _Options, JointCollectionTpl> (line 537) | struct SimulatorConstraintSolverTpl<::pinocchio::PGSContactSolverTpl...
FILE: include/simple/core/simulator.hxx
type simple (line 30) | namespace simple
type details (line 512) | namespace details
FILE: include/simple/fwd.hpp
type simple (line 39) | namespace simple
type traits (line 45) | struct traits
type context (line 49) | namespace context
FILE: include/simple/math/fwd.hpp
type simple (line 10) | namespace simple
FILE: include/simple/math/qr.hpp
type simple (line 12) | namespace simple
type math (line 14) | namespace math
type SolveInPlaceWrapper (line 17) | struct SolveInPlaceWrapper : _SolverType
method solveInPlace (line 22) | void solveInPlace(const Eigen::MatrixBase<MatrixType> & mat) const
type SolveInPlaceWrapper<Eigen::HouseholderQR<_MatrixType>> (line 32) | struct SolveInPlaceWrapper<Eigen::HouseholderQR<_MatrixType>> : Eige...
method solveInPlace (line 37) | void solveInPlace(const Eigen::MatrixBase<MatrixType> & mat_) const
FILE: include/simple/utils/visitors.hpp
type simple (line 12) | namespace simple
type visitors (line 14) | namespace visitors
type lambda_visitor_helper (line 18) | struct lambda_visitor_helper
type lambda_visitor_helper<ReturnType> (line 22) | struct lambda_visitor_helper<ReturnType>
method lambda_visitor_helper (line 25) | lambda_visitor_helper()
method ReturnType (line 30) | ReturnType operator()(const boost::blank &) const
type lambda_visitor (line 60) | struct lambda_visitor
method lambda_visitor (line 66) | lambda_visitor(Lambdas... lambdas)
function make_lambda_visitor (line 77) | lambda_visitor<ReturnType, Lambdas...> make_lambda_visitor(Lambdas.....
type lambda_visitor_helper<ReturnType, Lambda, Lambdas...> (line 39) | struct lambda_visitor_helper<ReturnType, Lambda, Lambdas...>
method lambda_visitor_helper (line 46) | lambda_visitor_helper(Lambda lambda, Lambdas... lambdas)
FILE: sandbox/cartpole.py
class SimulationArgs (line 13) | class SimulationArgs(tap.Tap):
function create_cartpole (line 45) | def create_cartpole(N, add_floor):
FILE: sandbox/go2_contact_id.py
class ScriptArgs (line 14) | class ScriptArgs(SimulationArgs):
function computeCost (line 120) | def computeCost(tau):
FILE: sandbox/parallel_rollout.py
function createSimulator (line 19) | def createSimulator(
function simulate_tau_batch_sequential (line 76) | def simulate_tau_batch_sequential(tau_batch, sim_batch, q, v, dt):
function simulate_tau_batch_parallel (line 93) | def simulate_tau_batch_parallel(sim_batch, q, v, tau_batch, dt):
FILE: sandbox/pin_utils.py
function addSystemCollisionPairs (line 5) | def addSystemCollisionPairs(model, geom_model, qref):
FILE: sandbox/sim_utils.py
class SimulationArgs (line 20) | class SimulationArgs(tap.Tap):
function setupSimulatorFromArgs (line 53) | def setupSimulatorFromArgs(sim: simple.Simulator, args: SimulationArgs):
function plotContactSolver (line 93) | def plotContactSolver(
function subSample (line 151) | def subSample(xs, duration, fps):
function addSystemCollisionPairs (line 162) | def addSystemCollisionPairs(model, geom_model, qref):
function addFloor (line 201) | def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryMo...
function addMaterialAndCompliance (line 217) | def addMaterialAndCompliance(geom_model, material: str, compliance: float):
function printSimulationPerfStats (line 236) | def printSimulationPerfStats(step_timings: np.ndarray):
function runMujocoXML (line 253) | def runMujocoXML(model_path: str, args: SimulationArgs):
function createVisualizer (line 328) | def createVisualizer(
FILE: sandbox/simulation_args.py
class SimulationArgs (line 4) | class SimulationArgs(tap.Tap):
method process_args (line 40) | def process_args(self):
class ControlArgs (line 48) | class ControlArgs(SimulationArgs):
FILE: sandbox/simulation_utils.py
class Policy (line 20) | class Policy:
method __init__ (line 21) | def __init__(self):
method act (line 24) | def act(
class DefaultPolicy (line 30) | class DefaultPolicy(Policy):
method __init__ (line 31) | def __init__(self, model: pin.Model):
method act (line 34) | def act(
class FreeFloatingRobotDampingPolicy (line 40) | class FreeFloatingRobotDampingPolicy(Policy):
method __init__ (line 41) | def __init__(self, model: pin.Model, damping_factor: float):
method act (line 46) | def act(
class RobotArmDampingPolicy (line 54) | class RobotArmDampingPolicy(Policy):
method __init__ (line 55) | def __init__(self, model: pin.Model, damping_factor: float):
method act (line 59) | def act(
function setPhysicsProperties (line 67) | def setPhysicsProperties(
function removeBVHModelsIfAny (line 86) | def removeBVHModelsIfAny(geom_model: pin.GeometryModel):
function addFloor (line 96) | def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryMo...
function simulateSytem (line 120) | def simulateSytem(
FILE: sandbox/test_memory.py
function createSimulator (line 54) | def createSimulator(
FILE: sandbox/viz_utils.py
function sub_sample (line 22) | def sub_sample(xs, duration, fps):
function rgbToHex (line 33) | def rgbToHex(color):
function register_object (line 44) | def register_object(
function register_line (line 64) | def register_line(
function register_arrowed_line (line 92) | def register_arrowed_line(
function transform_object (line 111) | def transform_object(
function delete_object (line 126) | def delete_object(viz: MeshcatVisualizer, name: str):
function create_arrow_head (line 133) | def create_arrow_head(scale_: float = 1.0, n: int = 10) -> hppfcl.Convex:
function npToTTuple (line 156) | def npToTTuple(M):
function npToTuple (line 163) | def npToTuple(M):
function load_primitive (line 173) | def load_primitive(geom: hppfcl.ShapeBase):
function loadMesh (line 238) | def loadMesh(mesh):
function createCapsule (line 276) | def createCapsule(length, radius, radial_resolution=30, cap_resolution=10):
class Plane (line 346) | class Plane(mg.Geometry):
method __init__ (line 349) | def __init__(
method lower (line 362) | def lower(self, object_data: Any) -> MsgType:
function meshcat_material (line 373) | def meshcat_material(r, g, b, a):
function create_visualizer (line 380) | def create_visualizer(
FILE: src/core/constraints-problem.cpp
type simple (line 7) | namespace simple
type ConstraintsProblemTpl<context::Scalar, context::Options, ::pinocchio::JointCollectionDefaultTpl> (line 10) | struct ConstraintsProblemTpl<context::Scalar, context::Options, ::pino...
FILE: src/core/simulator.cpp
type simple (line 7) | namespace simple
type SimulatorTpl<context::Scalar, context::Options, pinocchio::JointCollectionDefaultTpl> (line 9) | struct SimulatorTpl<context::Scalar, context::Options, pinocchio::Join...
FILE: src/pinocchio_template_instantiation/joint-model.cpp
type ::pinocchio::JointModelTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl> (line 4) | struct ::pinocchio::JointModelTpl<::simple::context::Scalar, ::simple::c...
type ::pinocchio::JointDataTpl<::simple::context::Scalar, ::simple::context::Options, ::pinocchio::JointCollectionDefaultTpl> (line 6) | struct ::pinocchio::JointDataTpl<::simple::context::Scalar, ::simple::co...
FILE: tests/forward/mujoco-humanoid.cpp
function addFloorToGeomModel (line 28) | void addFloorToGeomModel(GeometryModel & geom_model)
function addSystemCollisionPairs (line 39) | void addSystemCollisionPairs(const Model & model, GeometryModel & geom_m...
function BOOST_AUTO_TEST_CASE (line 87) | BOOST_AUTO_TEST_CASE(mujoco_humanoid)
FILE: tests/forward/simulation-combine-constraints.cpp
function createBallsScene (line 39) | std::tuple<ModelHandle, GeometryModelHandle, Eigen::VectorXd>
function compareConstraintsProblems (line 120) | void compareConstraintsProblems(const ConstraintsProblem & problem1, con...
function rand_interval (line 141) | double rand_interval(double rmin, double rmax)
function test_static_scene (line 155) | void test_static_scene(bool prismatic_joint)
function BOOST_AUTO_TEST_CASE (line 225) | BOOST_AUTO_TEST_CASE(simulator_instance_static_balls_using_joint)
function BOOST_AUTO_TEST_CASE (line 229) | BOOST_AUTO_TEST_CASE(simulator_instance_static_balls_using_freeflyer)
function test_moving_scene (line 238) | void test_moving_scene(bool prismatic_joint, bool compare_reset, bool in...
function BOOST_AUTO_TEST_CASE (line 317) | BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_joint)
function BOOST_AUTO_TEST_CASE (line 321) | BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_joint_compare...
function BOOST_AUTO_TEST_CASE (line 325) | BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer)
function BOOST_AUTO_TEST_CASE (line 329) | BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer_com...
function BOOST_AUTO_TEST_CASE (line 333) | BOOST_AUTO_TEST_CASE(simulator_instance_moving_balls_using_freeflyer_noi...
function test_constraint_scene (line 341) | void test_constraint_scene(bool prismatic_joint, bool floor_contact, boo...
function BOOST_AUTO_TEST_CASE (line 490) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_no_constraint)
function BOOST_AUTO_TEST_CASE (line 494) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_bilateral_constraint)
function BOOST_AUTO_TEST_CASE (line 498) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_constraint)
function BOOST_AUTO_TEST_CASE (line 502) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_constraint_ill_de...
function BOOST_AUTO_TEST_CASE (line 506) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_weld_and_bilat_constra...
function BOOST_AUTO_TEST_CASE (line 510) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_limit)
function BOOST_AUTO_TEST_CASE (line 514) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction)
function BOOST_AUTO_TEST_CASE (line 518) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_limit_static)
function BOOST_AUTO_TEST_CASE (line 522) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_static)
function BOOST_AUTO_TEST_CASE (line 526) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_joints_constraint_...
function BOOST_AUTO_TEST_CASE (line 530) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints)
function BOOST_AUTO_TEST_CASE (line 534) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_static)
function BOOST_AUTO_TEST_CASE (line 538) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_and_co...
function BOOST_AUTO_TEST_CASE (line 542) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_all_constraints_and_co...
function BOOST_AUTO_TEST_CASE (line 547) | BOOST_AUTO_TEST_CASE(simulator_instance_step_test_all_combination)
FILE: tests/forward/simulation-robots.cpp
function BOOST_AUTO_TEST_CASE (line 39) | BOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint)
function BOOST_AUTO_TEST_CASE (line 90) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_friction_on_joints)
function BOOST_AUTO_TEST_CASE (line 117) | BOOST_AUTO_TEST_CASE(simulator_instance_step_with_limits_on_joints_for_m...
FILE: tests/forward/simulator-minimal.cpp
function BOOST_AUTO_TEST_CASE (line 30) | BOOST_AUTO_TEST_CASE(humanoid_with_bilateral_constraint_minimal)
function BOOST_AUTO_TEST_CASE (line 105) | BOOST_AUTO_TEST_CASE(humanoid_with_weld_constraint_minimal)
FILE: tests/forward/simulator.cpp
function BOOST_AUTO_TEST_CASE (line 39) | BOOST_AUTO_TEST_CASE(simulator_instance_constructor)
function BOOST_AUTO_TEST_CASE (line 48) | BOOST_AUTO_TEST_CASE(simulator_instance_constructor_2)
function BOOST_AUTO_TEST_CASE (line 55) | BOOST_AUTO_TEST_CASE(simulator_instance_step)
function BOOST_AUTO_TEST_CASE (line 76) | BOOST_AUTO_TEST_CASE(simulator_instance_step_make_shared)
function BOOST_AUTO_TEST_CASE (line 97) | BOOST_AUTO_TEST_CASE(simulator_instance_step_cubes)
function BOOST_AUTO_TEST_CASE (line 162) | BOOST_AUTO_TEST_CASE(simulator_instance_checkCollisionPairs)
FILE: tests/forward/urdf-romeo.cpp
function BOOST_AUTO_TEST_CASE (line 39) | BOOST_AUTO_TEST_CASE(simulator_instance_romeo_step)
FILE: tests/python/test_simulator_instance.py
function setBallsAndHalfSpace (line 22) | def setBallsAndHalfSpace(length=[0.2], mass=[1.0]):
function test_init (line 74) | def test_init():
function test_init2 (line 83) | def test_init2():
function test_simulator_handles (line 90) | def test_simulator_handles():
function test_init_empty (line 128) | def test_init_empty():
function test_step (line 142) | def test_step():
function test_step_balls (line 161) | def test_step_balls():
function test_step_balls_with_constraints (line 204) | def test_step_balls_with_constraints():
function addSystemCollisionPairs (line 245) | def addSystemCollisionPairs(model, geom_model, qref):
function addFloor (line 284) | def addFloor(geom_model: pin.GeometryModel):
function test_manipulator_limits (line 291) | def test_manipulator_limits():
function test_humanoid_limits (line 307) | def test_humanoid_limits():
function test_freeflyer_limits (line 323) | def test_freeflyer_limits():
function test_composite_limits (line 348) | def test_composite_limits():
function test_mujoco_humanoid_limits (line 399) | def test_mujoco_humanoid_limits():
function setSolo (line 478) | def setSolo():
function test_step_solo (line 563) | def test_step_solo():
Condensed preview — 81 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (473K chars).
[
{
"path": ".clang-format",
"chars": 565,
"preview": "Language: Cpp\nBasedOnStyle: Microsoft\n\nAlwaysBreakTemplateDeclarations: Yes\nBinPackParameters: false\n\nBreakBeforeBinaryO"
},
{
"path": ".cmake-format.yaml",
"chars": 1047,
"preview": "markup:\n first_comment_is_literal: true\nformat:\n line_width: 100\nparse:\n additional_commands:\n cxx_flags_by_compil"
},
{
"path": ".gitignore",
"chars": 85,
"preview": "build*\nXcode*\n*~\n*.pyc\n**/.ipynb_checkpoints\ncoverage*\n.travis\n.vscode*\n*.orig\n.cache"
},
{
"path": ".gitmodules",
"chars": 93,
"preview": "[submodule \"cmake\"]\n\tpath = cmake\n\turl = https://github.com/jrl-umi3218/jrl-cmakemodules.git\n"
},
{
"path": ".pre-commit-config.yaml",
"chars": 1293,
"preview": "ci:\n autoupdate_branch: devel\n autofix_prs: false\nrepos:\n - repo: https://github.com/pre-commit/mirrors-clang-format\n"
},
{
"path": "CMakeLists.txt",
"chars": 10506,
"preview": "#\n# Copyright (c) 2024 INRIA\n#\n\ncmake_minimum_required(VERSION 3.10)\n\nset(PROJECT_NAME simple)\nset(PROJECT_DESCRIPTION \""
},
{
"path": "LICENSE",
"chars": 1492,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2025, INRIA\n\nRedistribution and use in source and binary forms, with or without\nmodi"
},
{
"path": "README.md",
"chars": 3625,
"preview": "# The Simple Simulator: Simulation Made Simple\n\n**Simple** is a new (differentiable) physical engine based on recent pro"
},
{
"path": "benchmark/CMakeLists.txt",
"chars": 458,
"preview": "add_project_private_dependency(benchmark REQUIRED)\n\nadd_custom_target(bench)\n\nfunction(ADD_SIMPLE_BENCHMARK benchmark_na"
},
{
"path": "benchmark/affine-transform.cpp",
"chars": 7071,
"preview": "#include <benchmark/benchmark.h>\n#include <Eigen/Core>\n\n#define N 100\n#define WARMUP_TIME 1.\n#define MIN_TIME 2.\n\nusing "
},
{
"path": "benchmark/mujoco-humanoid.cpp",
"chars": 5045,
"preview": "//\n// Copyright (c) 2025 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include \"tests/test_data/config.h\"\n\n#include <p"
},
{
"path": "bindings/CMakeLists.txt",
"chars": 163,
"preview": "#\n# Copyright (c) 2024 INRIA\n#\n\nadd_subdirectory(python)\nif(BUILD_PYTHON_INTERFACE)\n set(PYWRAP\n ${PYWRAP}\n P"
},
{
"path": "bindings/python/CMakeLists.txt",
"chars": 4654,
"preview": "#\n# Copyright (c) 2024 INRIA\n#\n\ninclude(${JRL_CMAKE_MODULES}/python-helpers.cmake)\ninclude(${JRL_CMAKE_MODULES}/stubs.cm"
},
{
"path": "bindings/python/core/expose-constraints-problem.cpp",
"chars": 1422,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/bindings/python/core/constraints-problem.hpp\"\n\nnamespace bp = boost:"
},
{
"path": "bindings/python/core/expose-contact-frame.cpp",
"chars": 1987,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/contact-frame.hpp\"\n#include \"simple/bindings/python/fwd.hpp\"\n\n#"
},
{
"path": "bindings/python/core/expose-simulator.cpp",
"chars": 269,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/bindings/python/core/simulator.hpp\"\n\nnamespace simple\n{\n namespace "
},
{
"path": "bindings/python/module.cpp",
"chars": 839,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/bindings/python/fwd.hpp\"\n#include \"simple/config.hpp\"\n#include \"simp"
},
{
"path": "bindings/python/simple/__init__.py",
"chars": 148,
"preview": "#\n# Copyright (c) 2024 INRIA\n#\n\nimport pinocchio\nimport hppfcl\nfrom .simple_pywrap import *\nfrom .simple_pywrap import _"
},
{
"path": "include/simple/bindings/python/core/constraints-problem.hpp",
"chars": 15828,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_python_core_constraints_problem_hpp__\n#define __simple_python_core_c"
},
{
"path": "include/simple/bindings/python/core/simulator.hpp",
"chars": 18708,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_python_algorithm_simulator_hpp__\n#define __simple_python_algorithm_s"
},
{
"path": "include/simple/bindings/python/fwd.hpp",
"chars": 365,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_python_fwd_hpp__\n#define __simple_python_fwd_hpp__\n\n#include <eigenp"
},
{
"path": "include/simple/core/constraints-problem.hpp",
"chars": 38229,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_constraints_problem_hpp__\n#define __simple_core_constraints_pro"
},
{
"path": "include/simple/core/constraints-problem.hxx",
"chars": 44194,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_constraints_problem_hxx__\n#define __simple_core_constraints_pro"
},
{
"path": "include/simple/core/constraints-problem.txx",
"chars": 402,
"preview": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#ifndef __simple_core_constraints_problem_txx__\n#define __simple_core_constraint"
},
{
"path": "include/simple/core/contact-frame.hpp",
"chars": 4889,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_contact_frame_hpp__\n#define __simple_core_contact_frame_hpp__\n\n"
},
{
"path": "include/simple/core/contact-frame.hxx",
"chars": 2724,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_contact_frame_hxx__\n#define __simple_core_contact_frame_hxx__\n\n"
},
{
"path": "include/simple/core/fwd.hpp",
"chars": 1581,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_fwd_hpp__\n#define __simple_core_fwd_hpp__\n\n#include \"simple/fwd"
},
{
"path": "include/simple/core/simulator.hpp",
"chars": 20960,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_simulator_hpp__\n#define __simple_core_simulator_hpp__\n\n#include"
},
{
"path": "include/simple/core/simulator.hxx",
"chars": 30603,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_core_simulator_hxx__\n#define __simple_core_simulator_hxx__\n\n#include"
},
{
"path": "include/simple/core/simulator.txx",
"chars": 2282,
"preview": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#ifndef __simple_simulator_txx__\n#define __simple_simulator_txx__\n\n#include \"sim"
},
{
"path": "include/simple/fwd.hpp",
"chars": 2097,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_fwd_hpp__\n#define __simple_fwd_hpp__\n\n#ifdef _WIN32\n #include <wind"
},
{
"path": "include/simple/math/fwd.hpp",
"chars": 210,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_math_fwd_hpp__\n#define __simple_math_fwd_hpp__\n\n#include \"simple/fwd"
},
{
"path": "include/simple/math/qr.hpp",
"chars": 1420,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_math_qr_hpp__\n#define __simple_math_qr_hpp__\n\n#include \"simple/math/"
},
{
"path": "include/simple/pch.hpp",
"chars": 1242,
"preview": "// PINOCCHIO\n#include <pinocchio/algorithm/aba-derivatives.hpp>\n#include <pinocchio/algorithm/fwd.hpp>\n#include <pinocch"
},
{
"path": "include/simple/pinocchio_template_instantiation/aba-derivatives.txx",
"chars": 1338,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_aba_derivatives_txx__\n#define __sim"
},
{
"path": "include/simple/pinocchio_template_instantiation/aba.txx",
"chars": 1440,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_aba_txx__\n#define __simple_pinocchi"
},
{
"path": "include/simple/pinocchio_template_instantiation/crba.txx",
"chars": 592,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_crba_txx__\n#define __simple_pinocch"
},
{
"path": "include/simple/pinocchio_template_instantiation/joint-model.txx",
"chars": 478,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_pinocchio_template_instantiation_joint_model_txx__\n#define __simple_"
},
{
"path": "include/simple/utils/visitors.hpp",
"chars": 2618,
"preview": "//\n// Copyright (c) 2025 INRIA\n//\n\n#ifndef __simple_utils_visitors_hpp__\n#define __simple_utils_visitors_hpp__\n\n#include"
},
{
"path": "sandbox/cartpole.py",
"chars": 8108,
"preview": "import pinocchio as pin\nimport numpy as np\nimport hppfcl\nimport tap\nfrom pinocchio.visualize import MeshcatVisualizer\nim"
},
{
"path": "sandbox/cassie_mj.py",
"chars": 4106,
"preview": "import pinocchio as pin\nimport numpy as np\nfrom simulation_args import SimulationArgs\nimport simple\nfrom robot_descripti"
},
{
"path": "sandbox/force_action_derivative.py",
"chars": 1059,
"preview": "import pinocchio as pin\nimport numpy as np\n\nnp.set_printoptions(suppress=True)\n\n# Testing the derivatives of M.act(f), w"
},
{
"path": "sandbox/four_bar_linkage.py",
"chars": 2934,
"preview": "import pinocchio as pin\nimport numpy as np\nimport simple\nimport time\nfrom sim_utils import (\n addSystemCollisionPairs"
},
{
"path": "sandbox/four_five_bar_linkage.py",
"chars": 2928,
"preview": "import pinocchio as pin\nimport numpy as np\nimport simple\nimport time\nfrom sim_utils import (\n addSystemCollisionPairs"
},
{
"path": "sandbox/go2_contact_id.py",
"chars": 6399,
"preview": "import pinocchio as pin\nimport numpy as np\nfrom pin_utils import addSystemCollisionPairs\nfrom simulation_utils import (\n"
},
{
"path": "sandbox/humanoid_mj.py",
"chars": 3374,
"preview": "import pinocchio as pin\nimport numpy as np\nimport simple\nfrom sim_utils import (\n # addFloor,\n addMaterialAndCompl"
},
{
"path": "sandbox/parallel_rollout.py",
"chars": 4425,
"preview": "import pinocchio as pin\nimport numpy as np\nimport concurrent.futures\nimport timeit\nimport simple\nimport os\nimport time\n\n"
},
{
"path": "sandbox/pendulum.py",
"chars": 2695,
"preview": "import pinocchio as pin\nimport numpy as np\nimport simple\nimport time\nfrom sim_utils import (\n addSystemCollisionPairs"
},
{
"path": "sandbox/pin_utils.py",
"chars": 2595,
"preview": "import hppfcl\nimport pinocchio as pin\n\n\ndef addSystemCollisionPairs(model, geom_model, qref):\n \"\"\"\n Add the right "
},
{
"path": "sandbox/robots/four_bar_linkage.xml",
"chars": 2321,
"preview": "<mujoco>\n <compiler inertiafromgeom=\"true\"/>\n <option timestep=\"0.002\" />\n <asset>\n <material name=\"blue"
},
{
"path": "sandbox/robots/four_five_bar_linkage.xml",
"chars": 3027,
"preview": "<mujoco>\n <compiler inertiafromgeom=\"true\"/>\n <option timestep=\"0.002\" />\n <asset>\n <material name=\"blue"
},
{
"path": "sandbox/robots/go2/mjcf/go2.xml",
"chars": 8727,
"preview": "<mujoco model=\"go2_description\">\n <compiler angle=\"radian\"/>\n <worldbody>\n <body name=\"base\">\n <inertial pos=\""
},
{
"path": "sandbox/robots/go2/mjcf/scene.xml",
"chars": 867,
"preview": "<mujoco model=\"Unitree Go2\">\n <include file=\"go2.xml\" />\n <statistic center=\"1 0.7 1.5\" extent=\"0.8\" />\n <visual>\n "
},
{
"path": "sandbox/robots/humanoid.xml",
"chars": 11472,
"preview": "<!-- Copyright 2021 DeepMind Technologies Limited\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n"
},
{
"path": "sandbox/robots/pendulum.xml",
"chars": 854,
"preview": "<mujoco>\n <compiler inertiafromgeom=\"true\"/>\n <option timestep=\"0.002\" />\n <asset>\n <material name=\"blue"
},
{
"path": "sandbox/sim_utils.py",
"chars": 13679,
"preview": "import pinocchio as pin\nimport hppfcl\nimport numpy as np\nimport tap\nimport simple\nimport matplotlib.pyplot as plt\nimport"
},
{
"path": "sandbox/simulation_args.py",
"chars": 1560,
"preview": "import tap\n\n\nclass SimulationArgs(tap.Tap):\n num_repetitions: int = 1\n display: bool = False\n display_com: bool"
},
{
"path": "sandbox/simulation_utils.py",
"chars": 23100,
"preview": "import time\nfrom typing import Dict\nimport numpy as np\nimport tap\nfrom viz_utils import RED, GREEN, BLUE, BLACK, PINK, G"
},
{
"path": "sandbox/test_memory.py",
"chars": 2714,
"preview": "import pinocchio as pin\nimport numpy as np\nfrom sim_utils import SimulationArgs, runMujocoXML\nimport mujoco\nimport os\n\na"
},
{
"path": "sandbox/viz_utils.py",
"chars": 12278,
"preview": "import hppfcl\nimport numpy as np\nimport meshcat\nimport meshcat.geometry as mg\nimport pinocchio as pin\nfrom pinocchio.vis"
},
{
"path": "sources.cmake",
"chars": 2636,
"preview": "# Define Simple sources and headers\n\n# Core library\nset(${PROJECT_NAME}_CORE_SOURCES empty.cpp)\n\nset(${PROJECT_NAME}_COR"
},
{
"path": "src/CMakeLists.txt",
"chars": 3947,
"preview": "#\n# Copyright (c) 2024 INRIA\n#\n\n# Create header-only target All other target will depend on it.\nadd_library(${PROJECT_NA"
},
{
"path": "src/core/constraints-problem.cpp",
"chars": 247,
"preview": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#include \"simple/core/constraints-problem.hpp\"\n\nnamespace simple\n{\n\n template s"
},
{
"path": "src/core/simulator.cpp",
"chars": 2130,
"preview": "//\n// Copyright (c) 2022-2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n\nnamespace simple\n{\n template struct Simul"
},
{
"path": "src/empty.cpp",
"chars": 0,
"preview": ""
},
{
"path": "src/pinocchio_template_instantiation/aba-derivatives.cpp",
"chars": 1189,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/fwd.hpp\"\n#include <pinocchio/algorithm/aba-derivatives.hpp>\n\ntemplat"
},
{
"path": "src/pinocchio_template_instantiation/aba.cpp",
"chars": 1328,
"preview": "#include \"simple/fwd.hpp\"\n#include <pinocchio/algorithm/aba.hpp>\n\ntemplate PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_D"
},
{
"path": "src/pinocchio_template_instantiation/crba.cpp",
"chars": 430,
"preview": "#include \"simple/fwd.hpp\"\n#include <pinocchio/algorithm/crba.hpp>\n\ntemplate PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_"
},
{
"path": "src/pinocchio_template_instantiation/joint-model.cpp",
"chars": 360,
"preview": "#include \"simple/fwd.hpp\"\n#include \"pinocchio/multibody/joint/joint-generic.hpp\"\n\ntemplate struct ::pinocchio::JointMode"
},
{
"path": "tests/CMakeLists.txt",
"chars": 2542,
"preview": "#\n# Copyright (c) 2024 INRIA\n#\n\n# Find Boost.UnitTestFramework\nfind_package(Boost COMPONENTS unit_test_framework)\n\n# Com"
},
{
"path": "tests/forward/CMakeLists.txt",
"chars": 270,
"preview": "add_simple_unit_test(mujoco-humanoid)\nadd_simple_unit_test(simulation-combine-constraints)\nadd_simple_unit_test(simulati"
},
{
"path": "tests/forward/mujoco-humanoid.cpp",
"chars": 5156,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include <pinocchio/algorithm/fwd.hpp>\n\n#include"
},
{
"path": "tests/forward/simulation-combine-constraints.cpp",
"chars": 21265,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include <pinocchio/algorithm/fwd.hpp>\n\n#include"
},
{
"path": "tests/forward/simulation-robots.cpp",
"chars": 6663,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include <pinocchio/algorithm/fwd.hpp>\n\n#include"
},
{
"path": "tests/forward/simulator-minimal.cpp",
"chars": 7226,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include <pinocchio/algorithm/fwd.hpp>\n\n#include"
},
{
"path": "tests/forward/simulator.cpp",
"chars": 7752,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include <pinocchio/algorithm/fwd.hpp>\n\n#include"
},
{
"path": "tests/forward/urdf-romeo.cpp",
"chars": 3014,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#include \"simple/core/simulator.hpp\"\n#include <pinocchio/algorithm/fwd.hpp>\n\n#include"
},
{
"path": "tests/python/test_simulator_instance.py",
"chars": 20626,
"preview": "import pinocchio as pin\nimport simple\nimport numpy as np\nfrom hppfcl import (\n Halfspace,\n Sphere,\n HeightField"
},
{
"path": "tests/test-utils.hpp",
"chars": 1674,
"preview": "#ifndef __simple__test_utils_hpp__\n#define __simple__test_utils_hpp__\n\n#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)"
},
{
"path": "tests/test_data/config.h.in",
"chars": 222,
"preview": "//\n// Copyright (c) 2024 INRIA\n//\n\n#ifndef __simple_test_data_config_h__\n#define __simple_test_data_config_h__\n\n#define "
},
{
"path": "tests/test_data/mujoco_humanoid.xml",
"chars": 11207,
"preview": "<!-- Copyright 2021 DeepMind Technologies Limited\n\n Licensed under the Apache License, Version 2.0 (the \"License\");\n"
}
]
About this extraction
This page contains the full source code of the Simple-Robotics/Simple GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 81 files (441.5 KB), approximately 117.2k tokens, and a symbol index with 292 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.