├── .gitattributes ├── .gitmodules ├── development ├── scripts │ └── pixi │ │ ├── activation.bat │ │ └── activation.sh └── release.md ├── .gitignore ├── .github ├── dependabot.yml └── workflows │ ├── check-changelog.yml │ ├── update_pixi_lockfile.yml │ └── macos-linux-windows-pixi.yml ├── .git-blame-ignore-revs ├── src ├── loik-loid.cpp ├── loik-loid-optimized.cpp ├── loik-loid-data.cpp └── loik-loid-data-optimized.cpp ├── include └── loik │ ├── loik-loid.txx │ ├── loik-loid-optimized.txx │ ├── macros.hpp │ ├── fwd.hpp │ ├── loik-loid-data.txx │ ├── loik-loid-data-optimized.txx │ ├── task-solver-base.hpp │ ├── loik-loid-data.hxx │ ├── loik-loid-data.hpp │ ├── loik-loid-data-optimized.hxx │ ├── ik-id-description-optimized.hpp │ ├── loik-loid-data-optimized.hpp │ ├── loik-loid.hxx │ ├── ik-id-description.hpp │ └── loik-loid.hpp ├── .cmake-format.yaml ├── .clang-format ├── CHANGELOG.md ├── .pre-commit-config.yaml ├── LICENSE ├── tests ├── CMakeLists.txt └── loik-loid-data.cpp ├── pixi.toml ├── README.md └── CMakeLists.txt /.gitattributes: -------------------------------------------------------------------------------- 1 | # GitHub syntax highlighting 2 | pixi.lock linguist-language=YAML 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules 4 | -------------------------------------------------------------------------------- /development/scripts/pixi/activation.bat: -------------------------------------------------------------------------------- 1 | :: Set default build value only if not previously set 2 | if not defined LOIK_BUILD_TYPE (set LOIK_BUILD_TYPE=Release) 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | Xcode* 3 | *~ 4 | *.pyc 5 | **/.ipynb_checkpoints 6 | coverage* 7 | .vscode* 8 | *.orig 9 | # pixi environments 10 | .pixi 11 | *.egg-info 12 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: github-actions 4 | directory: / 5 | target-branch: devel 6 | schedule: 7 | interval: weekly 8 | -------------------------------------------------------------------------------- /.git-blame-ignore-revs: -------------------------------------------------------------------------------- 1 | # pre-commit run -a (format-all, 2024-07-18) 2 | 063302c38bb7850f0d3ce512c7ecfd726b0abacf 3 | # pre-commit run -a (format-all, 2024-07-24) 4 | 47e8fd6ab01fe4cdeb669ed020571cc9d7de16c3 5 | -------------------------------------------------------------------------------- /src/loik-loid.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "loik/loik-loid.hpp" 6 | 7 | namespace loik 8 | { 9 | 10 | template struct LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI 11 | FirstOrderLoikTpl; 12 | 13 | } // namespace loik 14 | -------------------------------------------------------------------------------- /include/loik/loik-loid.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid.hpp" 8 | #include "loik/loik-loid-optimized.hpp" 9 | 10 | namespace loik 11 | { 12 | extern template struct LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI 13 | FirstOrderLoikTpl; 14 | } // namespace loik 15 | -------------------------------------------------------------------------------- /src/loik-loid-optimized.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "loik/loik-loid-optimized.hpp" 6 | 7 | namespace loik 8 | { 9 | 10 | template struct LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI FirstOrderLoikOptimizedTpl< 11 | pinocchio::context::Scalar, 12 | pinocchio::context::Options, 13 | pinocchio::JointCollectionDefaultTpl>; 14 | 15 | } // namespace loik 16 | -------------------------------------------------------------------------------- /include/loik/loik-loid-optimized.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid-optimized.hpp" 8 | 9 | namespace loik 10 | { 11 | extern template struct LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI FirstOrderLoikOptimizedTpl< 12 | pinocchio::context::Scalar, 13 | pinocchio::context::Options, 14 | pinocchio::JointCollectionDefaultTpl>; 15 | } // namespace loik 16 | -------------------------------------------------------------------------------- /.github/workflows/check-changelog.yml: -------------------------------------------------------------------------------- 1 | name: CI - Check-changelog 2 | 3 | on: 4 | pull_request: 5 | types: [assigned, opened, synchronize, reopened, labeled, unlabeled, edited] 6 | branches: 7 | - devel 8 | jobs: 9 | check-changelog: 10 | name: Check changelog action 11 | runs-on: ubuntu-latest 12 | steps: 13 | - uses: tarides/changelog-check-action@v3 14 | with: 15 | changelog: CHANGELOG.md 16 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | markup: 2 | first_comment_is_literal: true 3 | format: 4 | line_width: 100 5 | parse: 6 | additional_commands: 7 | cxx_flags_by_compiler_frontend: 8 | pargs: 0 9 | kwargs: 10 | OUTPUT: 1 11 | GNU: '*' 12 | MSVC: '*' 13 | FILTER: 0 14 | add_project_dependency: 15 | pargs: '*' 16 | kwargs: 17 | PKG_CONFIG_REQUIRES: 1 18 | FOR_COMPONENT: 1 19 | FIND_EXTERNAL: 1 20 | -------------------------------------------------------------------------------- /include/loik/macros.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #ifdef LOIK_EIGEN_CHECK_MALLOC 8 | #define LOIK_EIGEN_ALLOW_MALLOC(allowed) ::Eigen::internal::set_is_malloc_allowed(allowed) 9 | #define LOIK_EIGEN_MALLOC_ALLOWED() LOIK_EIGEN_ALLOW_MALLOC(true) 10 | #define LOIK_EIGEN_MALLOC_NOT_ALLOWED() LOIK_EIGEN_ALLOW_MALLOC(false) 11 | #else 12 | #define LOIK_EIGEN_ALLOW_MALLOC(allowed) 13 | #define LOIK_EIGEN_MALLOC_ALLOWED() 14 | #define LOIK_EIGEN_MALLOC_NOT_ALLOWED() 15 | #endif 16 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Microsoft 3 | 4 | AlwaysBreakTemplateDeclarations: Yes 5 | BinPackParameters: false 6 | 7 | BreakBeforeBinaryOperators: NonAssignment 8 | BreakConstructorInitializers: BeforeComma 9 | BreakInheritanceList: BeforeComma 10 | 11 | ConstructorInitializerIndentWidth: 0 12 | ContinuationIndentWidth: 2 13 | 14 | IndentWidth: 2 15 | 16 | NamespaceIndentation: All 17 | 18 | PackConstructorInitializers: Never 19 | 20 | PenaltyReturnTypeOnItsOwnLine: 10 21 | PointerAlignment: Middle 22 | 23 | SpaceAfterTemplateKeyword: false 24 | 25 | ColumnLimit: 100 26 | 27 | SortIncludes: false 28 | 29 | IndentPPDirectives: BeforeHash 30 | 31 | AlignAfterOpenBracket: AlwaysBreak 32 | -------------------------------------------------------------------------------- /development/release.md: -------------------------------------------------------------------------------- 1 | # Release with Pixi 2 | 3 | To create a release with Pixi run the following commands on the **devel** branch: 4 | 5 | ```bash 6 | LOIK_VERSION=X.Y.Z pixi run release-new-version 7 | git push origin 8 | git push origin vX.Y.Z 9 | ``` 10 | 11 | Where `X.Y.Z` is the new version. 12 | Be careful to follow the [Semantic Versioning](https://semver.org/spec/v2.0.0.html) rules. 13 | 14 | You will find the following assets: 15 | - `./build_new_version/loik-X.Y.Z.tar.gz` 16 | - `./build_new_version/loik-X.Y.Z.tar.gz.sig` 17 | 18 | Then, create a new release on [GitHub](https://github.com/Simple-Robotics/LoIK/releases/new) with: 19 | 20 | * Tag: vX.Y.Z 21 | * Title: LoIK X.Y.Z 22 | * Body: 23 | ``` 24 | ## What's Changed 25 | 26 | CHANGELOG CONTENT 27 | 28 | **Full Changelog**: https://github.com/Simple-Robotics/LoIK/compare/vXX.YY.ZZ...vX.Y.Z 29 | ``` 30 | 31 | Where `XX.YY.ZZ` is the last release version. 32 | 33 | Then upload `loik-X.Y.Z.tar.gz` and `loik-X.Y.Z.tar.gz.sig` and publish the release. 34 | -------------------------------------------------------------------------------- /include/loik/fwd.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include 6 | 7 | #pragma once 8 | 9 | namespace loik 10 | { 11 | 12 | // IkIdDataTpl 13 | template< 14 | typename _Scalar, 15 | int _Options = 0, 16 | template class JointCollectionTpl = pinocchio::JointCollectionDefaultTpl> 17 | struct IkIdDataTpl; 18 | 19 | // IkIdDataTypeOptimizeTpl 20 | template< 21 | typename _Scalar, 22 | int _Options = 0, 23 | template class JointCollectionTpl = pinocchio::JointCollectionDefaultTpl> 24 | struct IkIdDataTypeOptimizedTpl; 25 | 26 | // IkIdSolverBaseTpl 27 | template 28 | struct IkIdSolverBaseTpl; 29 | 30 | // FirstOrderLoikTpl 31 | template 32 | struct FirstOrderLoikTpl; 33 | 34 | // FirstOrderLoikOptimizedTpl 35 | template< 36 | typename _Scalar, 37 | int _Options = 0, 38 | template class JointCollectionTpl = pinocchio::JointCollectionDefaultTpl> 39 | struct FirstOrderLoikOptimizedTpl; 40 | 41 | } // namespace loik 42 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | All notable changes to this project will be documented in this file. 4 | 5 | The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), 6 | and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). 7 | 8 | ## [Unreleased] 9 | 10 | ### Changed 11 | - Formating of the project 12 | - Python version update ([#17](https://github.com/Simple-Robotics/LoIK/pull/17)): 13 | - Project is now tested with Python 3.10 and 3.14 14 | - Python 3.10 is the minimal supported Python version 15 | 16 | ### Added 17 | - Add compatibility with jrl-cmakemodules workspace ([#2](https://github.com/Simple-Robotics/LoIK/pull/2)) 18 | - Add pixi-build support ([#17](https://github.com/Simple-Robotics/LoIK/pull/17)) 19 | 20 | ### Fixed 21 | - Remove CMake CMP0167 warnings ([#7](https://github.com/Simple-Robotics/LoIK/pull/7)) 22 | - Remove pixi 0.57 warnings ([#17](https://github.com/Simple-Robotics/LoIK/pull/17)) 23 | 24 | ## [1.0.0] - 2024-07-17 25 | 26 | ### Added 27 | 28 | - First public version 29 | 30 | [unreleased]: https://github.com/Simple-Robotics/LoIK/compare/v1.0.0...HEAD 31 | [1.0.0]: https://github.com/Simple-Robotics/LoIK/releases/releases/tag/v1.0.0 32 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | ci: 2 | autoupdate_branch: devel 3 | autofix_prs: false 4 | autoupdate_schedule: quarterly 5 | repos: 6 | - repo: https://github.com/pre-commit/mirrors-clang-format 7 | rev: v21.1.2 8 | hooks: 9 | - id: clang-format 10 | types_or: [] 11 | types: [text] 12 | files: \.(cpp|cxx|c|h|hpp|hxx|txx)$ 13 | - repo: https://github.com/pre-commit/pre-commit-hooks 14 | rev: v6.0.0 15 | hooks: 16 | - id: check-added-large-files 17 | - id: check-case-conflict 18 | - id: detect-private-key 19 | - id: end-of-file-fixer 20 | - id: mixed-line-ending 21 | - id: check-merge-conflict 22 | - id: trailing-whitespace 23 | - repo: https://github.com/cheshirekow/cmake-format-precommit 24 | rev: v0.6.13 25 | hooks: 26 | - id: cmake-format 27 | additional_dependencies: [pyyaml>=5.1] 28 | - repo: https://github.com/Lucas-C/pre-commit-hooks 29 | rev: v1.5.5 30 | hooks: 31 | - id: forbid-tabs 32 | - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt 33 | rev: 0.2.3 34 | hooks: 35 | - id: yamlfmt 36 | args: [--mapping=2, --offset=2, --sequence=4, --implicit_start, --preserve-quotes] 37 | -------------------------------------------------------------------------------- /src/loik-loid-data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "loik/loik-loid-data.hpp" 6 | 7 | namespace loik 8 | { 9 | 10 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI IkIdDataTpl< 11 | pinocchio::context::Scalar, 12 | pinocchio::context::Options, 13 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTpl(const Model &); 14 | 15 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI IkIdDataTpl< 16 | pinocchio::context::Scalar, 17 | pinocchio::context::Options, 18 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTpl(const Model &, const int); 19 | 20 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void IkIdDataTpl< 21 | pinocchio::context::Scalar, 22 | pinocchio::context::Options, 23 | pinocchio::JointCollectionDefaultTpl>::Reset(bool); 24 | 25 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void IkIdDataTpl< 26 | pinocchio::context::Scalar, 27 | pinocchio::context::Options, 28 | pinocchio::JointCollectionDefaultTpl>::ResetRecursion(); 29 | 30 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void IkIdDataTpl< 31 | pinocchio::context::Scalar, 32 | pinocchio::context::Options, 33 | pinocchio::JointCollectionDefaultTpl>::UpdatePrev(); 34 | 35 | } // namespace loik 36 | -------------------------------------------------------------------------------- /development/scripts/pixi/activation.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | # Remove flags setup from cxx-compiler 3 | unset CFLAGS 4 | unset CPPFLAGS 5 | unset CXXFLAGS 6 | unset DEBUG_CFLAGS 7 | unset DEBUG_CPPFLAGS 8 | unset DEBUG_CXXFLAGS 9 | unset LDFLAGS 10 | 11 | if [[ $host_alias == *"apple"* ]]; 12 | then 13 | # On OSX setting the rpath and -L it's important to use the conda libc++ instead of the system one. 14 | # If conda-forge use install_name_tool to package some libs, -headerpad_max_install_names is then mandatory 15 | export LDFLAGS="-Wl,-headerpad_max_install_names -Wl,-rpath,$CONDA_PREFIX/lib -L$CONDA_PREFIX/lib" 16 | elif [[ $host_alias == *"linux"* ]]; 17 | then 18 | # On GNU/Linux, I don't know if these flags are mandatory with g++ but 19 | # it allow to use clang++ as compiler 20 | export LDFLAGS="-Wl,-rpath,$CONDA_PREFIX/lib -Wl,-rpath-link,$CONDA_PREFIX/lib -L$CONDA_PREFIX/lib" 21 | 22 | # Conda compiler is named x86_64-conda-linux-gnu-c++, ccache can't resolve it 23 | # (https://ccache.dev/manual/latest.html#config_compiler_type) 24 | export CCACHE_COMPILERTYPE=gcc 25 | fi 26 | # Without -isystem, some LSP can't find headers 27 | export LOIK_CXX_FLAGS="$CXXFLAGS $LOIK_CXX_FLAGS -isystem $CONDA_PREFIX/include" 28 | 29 | # Set default build value only if not previously set 30 | export LOIK_BUILD_TYPE=${LOIK_BUILD_TYPE:=Release} 31 | -------------------------------------------------------------------------------- /include/loik/loik-loid-data.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid-data.hpp" 8 | 9 | namespace loik 10 | { 11 | 12 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI IkIdDataTpl< 13 | pinocchio::context::Scalar, 14 | pinocchio::context::Options, 15 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTpl(const Model &); 16 | 17 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI IkIdDataTpl< 18 | pinocchio::context::Scalar, 19 | pinocchio::context::Options, 20 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTpl(const Model &, const int); 21 | 22 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void IkIdDataTpl< 23 | pinocchio::context::Scalar, 24 | pinocchio::context::Options, 25 | pinocchio::JointCollectionDefaultTpl>::Reset(bool); 26 | 27 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void IkIdDataTpl< 28 | pinocchio::context::Scalar, 29 | pinocchio::context::Options, 30 | pinocchio::JointCollectionDefaultTpl>::ResetRecursion(); 31 | 32 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void IkIdDataTpl< 33 | pinocchio::context::Scalar, 34 | pinocchio::context::Options, 35 | pinocchio::JointCollectionDefaultTpl>::UpdatePrev(); 36 | 37 | } // namespace loik 38 | -------------------------------------------------------------------------------- /src/loik-loid-data-optimized.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "loik/loik-loid-data-optimized.hpp" 6 | 7 | namespace loik 8 | { 9 | 10 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI IkIdDataTypeOptimizedTpl< 11 | pinocchio::context::Scalar, 12 | pinocchio::context::Options, 13 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTypeOptimizedTpl(const Model &); 14 | 15 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI IkIdDataTypeOptimizedTpl< 16 | pinocchio::context::Scalar, 17 | pinocchio::context::Options, 18 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTypeOptimizedTpl(const Model &, const int); 19 | 20 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void IkIdDataTypeOptimizedTpl< 21 | pinocchio::context::Scalar, 22 | pinocchio::context::Options, 23 | pinocchio::JointCollectionDefaultTpl>::Reset(bool); 24 | 25 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void IkIdDataTypeOptimizedTpl< 26 | pinocchio::context::Scalar, 27 | pinocchio::context::Options, 28 | pinocchio::JointCollectionDefaultTpl>::ResetRecursion(); 29 | 30 | template LOIK_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void IkIdDataTypeOptimizedTpl< 31 | pinocchio::context::Scalar, 32 | pinocchio::context::Options, 33 | pinocchio::JointCollectionDefaultTpl>::UpdatePrev(); 34 | 35 | } // namespace loik 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2024, Inria 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_project_dependency(Boost COMPONENTS unit_test_framework REQUIRED) 2 | 3 | add_project_private_dependency(example-robot-data REQUIRED) 4 | 5 | function(add_loik_test name) 6 | set(test_name "${PROJECT_NAME}-test-cpp-${name}") 7 | set(test_file ${name}.cpp) 8 | 9 | add_unit_test(${test_name} ${test_file}) 10 | set_standard_output_directory(${test_name}) 11 | set_target_properties(${test_name} PROPERTIES LINKER_LANGUAGE CXX) 12 | 13 | # define macros required by boost_test see: 14 | # https://www.boost.org/doc/libs/1_78_0/libs/test/doc/html/boost_test/usage_variants.html define 15 | # module name, replace '-' by '_' 16 | set(MODULE_NAME "${name}Test") 17 | string(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME}) 18 | 19 | target_compile_definitions(${test_name} PRIVATE BOOST_TEST_DYN_LINK 20 | BOOST_TEST_MODULE=${MODULE_NAME}) 21 | 22 | target_link_libraries(${test_name} PRIVATE ${PROJECT_NAME} Boost::unit_test_framework 23 | example-robot-data::example-robot-data) 24 | endfunction() 25 | 26 | add_loik_test(loik-loid) 27 | 28 | string(TOUPPER "${CMAKE_BUILD_TYPE}" uppercase_CMAKE_BUILD_TYPE) 29 | if(uppercase_CMAKE_BUILD_TYPE STREQUAL DEBUG) 30 | set_tests_properties(${PROJECT_NAME}-test-cpp-loik-loid PROPERTIES TIMEOUT 3000) 31 | endif() 32 | 33 | add_loik_test(loik-loid-data) 34 | -------------------------------------------------------------------------------- /include/loik/loik-loid-data-optimized.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid-data-optimized.hpp" 8 | 9 | namespace loik 10 | { 11 | 12 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI IkIdDataTypeOptimizedTpl< 13 | pinocchio::context::Scalar, 14 | pinocchio::context::Options, 15 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTypeOptimizedTpl(const Model &); 16 | 17 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI IkIdDataTypeOptimizedTpl< 18 | pinocchio::context::Scalar, 19 | pinocchio::context::Options, 20 | pinocchio::JointCollectionDefaultTpl>::IkIdDataTypeOptimizedTpl(const Model &, const int); 21 | 22 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void IkIdDataTypeOptimizedTpl< 23 | pinocchio::context::Scalar, 24 | pinocchio::context::Options, 25 | pinocchio::JointCollectionDefaultTpl>::Reset(bool); 26 | 27 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void IkIdDataTypeOptimizedTpl< 28 | pinocchio::context::Scalar, 29 | pinocchio::context::Options, 30 | pinocchio::JointCollectionDefaultTpl>::ResetRecursion(); 31 | 32 | extern template LOIK_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void IkIdDataTypeOptimizedTpl< 33 | pinocchio::context::Scalar, 34 | pinocchio::context::Options, 35 | pinocchio::JointCollectionDefaultTpl>::UpdatePrev(); 36 | 37 | } // namespace loik 38 | -------------------------------------------------------------------------------- /.github/workflows/update_pixi_lockfile.yml: -------------------------------------------------------------------------------- 1 | name: CI - Update Pixi lockfile 2 | permissions: 3 | contents: write 4 | pull-requests: write 5 | 6 | on: 7 | workflow_dispatch: 8 | schedule: 9 | - cron: 0 5 1 * * 10 | 11 | jobs: 12 | pixi-update: 13 | runs-on: ubuntu-latest 14 | 15 | steps: 16 | - uses: actions/create-github-app-token@v2 17 | id: generate-token 18 | with: 19 | app-id: ${{ secrets.APP_ID }} 20 | private-key: ${{ secrets.APP_PRIVATE_KEY }} 21 | 22 | - uses: actions/checkout@v6 23 | with: 24 | token: ${{ steps.generate-token.outputs.token }} 25 | ref: devel 26 | # Make sure the value of GITHUB_TOKEN will not be persisted in repo's config 27 | persist-credentials: false 28 | 29 | - name: Set up pixi 30 | uses: prefix-dev/setup-pixi@v0.9.3 31 | with: 32 | run-install: false 33 | 34 | - name: Update lockfile 35 | run: | 36 | set -o pipefail 37 | pixi update --json | pixi exec pixi-diff-to-markdown >> diff.md 38 | 39 | - name: Create pull request 40 | uses: peter-evans/create-pull-request@v8 41 | with: 42 | token: ${{ steps.generate-token.outputs.token }} 43 | commit-message: 'pixi: Update pixi lockfile' 44 | title: Update pixi lockfile 45 | body-path: diff.md 46 | branch: topic/update-pixi 47 | base: devel 48 | labels: | 49 | pixi 50 | no changelog 51 | delete-branch: true 52 | add-paths: pixi.lock 53 | -------------------------------------------------------------------------------- /tests/loik-loid-data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "loik/loik-loid-data.hpp" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 18 | 19 | using Scalar = double; 20 | using Model = pinocchio::ModelTpl; 21 | using Data = pinocchio::DataTpl; 22 | using IkIdData = loik::IkIdDataTpl; 23 | 24 | BOOST_AUTO_TEST_CASE(test_loik_loid_data_creation) 25 | { 26 | // empty model data 27 | Model empty_model; 28 | IkIdData empty_data(empty_model); 29 | 30 | // humanoid model data 31 | Model humanoid_model; 32 | pinocchio::buildModels::humanoidRandom(humanoid_model); 33 | Data humanoid_data(humanoid_model); 34 | IkIdData humanoid_ikid_data(humanoid_model); 35 | 36 | // checkIkIdData(humanoid_model, humanoid_ikid_data, humanoid_data); 37 | 38 | // std::cout << checkIkIdData(humanoid_model, humanoid_ikid_data, 39 | // humanoid_data) << std::endl; 40 | 41 | // test against pinocchio data 42 | BOOST_CHECK(checkIkIdData(humanoid_model, humanoid_ikid_data)); 43 | BOOST_CHECK(checkIkIdData(humanoid_model, humanoid_ikid_data, humanoid_data)); 44 | } 45 | 46 | BOOST_AUTO_TEST_CASE(test_loik_loid_data_copy_and_equal_op) 47 | { 48 | Model humanoid_model; 49 | pinocchio::buildModels::humanoidRandom(humanoid_model); 50 | 51 | Data humanoid_data(humanoid_model); 52 | IkIdData humanoid_ikid_data(humanoid_model); 53 | IkIdData humanoid_ikid_data_copy = humanoid_ikid_data; 54 | 55 | // check ik_id_data against itself and its copy 56 | BOOST_CHECK(humanoid_ikid_data == humanoid_ikid_data); 57 | BOOST_CHECK(humanoid_ikid_data == humanoid_ikid_data_copy); 58 | 59 | // check ik_id_data_copy against data 60 | BOOST_CHECK(humanoid_ikid_data_copy == humanoid_data); 61 | BOOST_CHECK(humanoid_data == humanoid_ikid_data_copy); 62 | 63 | // 64 | humanoid_ikid_data_copy.oMi[0].setRandom(); 65 | BOOST_CHECK(humanoid_ikid_data != humanoid_ikid_data_copy); 66 | BOOST_CHECK(humanoid_ikid_data_copy != humanoid_ikid_data); 67 | BOOST_CHECK(humanoid_ikid_data_copy != humanoid_data); 68 | BOOST_CHECK(humanoid_data != humanoid_ikid_data_copy); 69 | } 70 | 71 | BOOST_AUTO_TEST_CASE(test_container_aligned_vector) 72 | { 73 | Model model; 74 | pinocchio::buildModels::humanoidRandom(model); 75 | 76 | IkIdData ikid_data(model); 77 | 78 | pinocchio::container::aligned_vector & vis = ikid_data.vis; 79 | PINOCCHIO_ALIGNED_STD_VECTOR(IkIdData::Motion) & vis_using_macro = ikid_data.vis; 80 | 81 | ikid_data.vis[0].setRandom(); 82 | BOOST_CHECK(vis[0] == ikid_data.vis[0]); 83 | BOOST_CHECK(vis_using_macro[0] == ikid_data.vis[0]); 84 | } 85 | 86 | BOOST_AUTO_TEST_CASE(test_std_vector_of_Data) 87 | { 88 | Model model; 89 | pinocchio::buildModels::humanoidRandom(model); 90 | 91 | PINOCCHIO_ALIGNED_STD_VECTOR(IkIdData) ikid_datas; 92 | 93 | for (size_t k = 0; k < 20; ++k) 94 | ikid_datas.push_back(IkIdData(model)); 95 | } 96 | 97 | BOOST_AUTO_TEST_SUITE_END() 98 | -------------------------------------------------------------------------------- /.github/workflows/macos-linux-windows-pixi.yml: -------------------------------------------------------------------------------- 1 | name: CI - MacOS/Linux/Windows via Pixi 2 | 3 | on: 4 | push: 5 | branches: 6 | - devel 7 | paths-ignore: 8 | - .gitignore 9 | - '*.md' 10 | - LICENSE 11 | - .pre-commit-config.yaml 12 | pull_request: 13 | paths-ignore: 14 | - .gitignore 15 | - '*.md' 16 | - LICENSE 17 | - .pre-commit-config.yaml 18 | concurrency: 19 | group: ${{ github.workflow }}-${{ github.ref }} 20 | cancel-in-progress: true 21 | 22 | jobs: 23 | loik-pixi: 24 | name: Standard - ${{ matrix.os }} - Env ${{ matrix.environment }} ${{ matrix.build_type }} 25 | runs-on: ${{ matrix.os }} 26 | env: 27 | CCACHE_BASEDIR: ${GITHUB_WORKSPACE} 28 | CCACHE_DIR: ${GITHUB_WORKSPACE}/.ccache 29 | CCACHE_COMPRESS: true 30 | CCACHE_COMPRESSLEVEL: 6 31 | # Since pixi will install a compiler, the compiler mtime will be changed. 32 | # This can invalidate the cache (https://ccache.dev/manual/latest.html#config_compiler_check) 33 | CCACHE_COMPILERCHECK: content 34 | 35 | strategy: 36 | fail-fast: false 37 | matrix: 38 | os: [ubuntu-latest, macos-latest, macos-15-intel] 39 | environment: [default, python-oldest] 40 | build_type: [Release, Debug] 41 | 42 | include: 43 | - os: windows-latest 44 | build_type: Release 45 | environment: default 46 | - os: windows-latest 47 | build_type: Release 48 | environment: python-oldest 49 | - os: windows-latest 50 | build_type: Release 51 | environment: clang-cl 52 | 53 | steps: 54 | - uses: actions/checkout@v6 55 | with: 56 | submodules: recursive 57 | 58 | - uses: actions/cache@v5 59 | with: 60 | path: .ccache 61 | key: ccache-macos-linux-windows-pixi-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.environment }}-${{ github.sha }} 62 | restore-keys: ccache-macos-linux-windows-pixi-${{ matrix.os }}-${{ matrix.build_type }}-${{ matrix.environment }}- 63 | 64 | - uses: prefix-dev/setup-pixi@v0.9.3 65 | with: 66 | cache: true 67 | environments: ${{ matrix.environment }} 68 | 69 | - name: Clear ccache statistics [MacOS/Linux/Windows] 70 | run: | 71 | pixi run -e ${{ matrix.environment }} ccache -z 72 | 73 | - name: Build LoIK [MacOS/Linux/Windows] 74 | shell: bash -el {0} 75 | env: 76 | LOIK_BUILD_TYPE: ${{ matrix.build_type }} 77 | CMAKE_BUILD_PARALLEL_LEVEL: 4 78 | run: | 79 | pixi run -e ${{ matrix.environment }} test 80 | 81 | - name: Display ccache statistics [MacOS/Linux/Windows] 82 | shell: bash -el {0} 83 | run: | 84 | pixi run -e ${{ matrix.environment }} ccache -sv 85 | 86 | loik-pixi-build: 87 | name: Pixi build - ${{ matrix.os }} 88 | runs-on: ${{ matrix.os }} 89 | 90 | strategy: 91 | fail-fast: false 92 | matrix: 93 | os: [ubuntu-latest, macos-latest, macos-15-intel, windows-latest] 94 | 95 | steps: 96 | - uses: actions/checkout@v6 97 | with: 98 | submodules: recursive 99 | 100 | - uses: prefix-dev/setup-pixi@v0.9.3 101 | env: 102 | CMAKE_BUILD_PARALLEL_LEVEL: 4 103 | with: 104 | cache: true 105 | environments: test-pixi-build 106 | 107 | - name: Test package [MacOS/Linux/Windows] 108 | run: | 109 | pixi run -e test-pixi-build test 110 | 111 | check: 112 | if: always() 113 | name: check-macos-linux-windows-pixi 114 | 115 | needs: 116 | - loik-pixi 117 | - loik-pixi-build 118 | 119 | runs-on: Ubuntu-latest 120 | 121 | steps: 122 | - name: Decide whether the needed jobs succeeded or failed 123 | uses: re-actors/alls-green@release/v1 124 | with: 125 | jobs: ${{ toJSON(needs) }} 126 | -------------------------------------------------------------------------------- /pixi.toml: -------------------------------------------------------------------------------- 1 | [workspace] 2 | name = "loik" 3 | version = "1.0.0" 4 | description = "LoIK is a simple yet efficient (constrained) differential inverse kinematics solver for robotics" 5 | authors = ["Bruce Wingo "] 6 | channels = ["conda-forge"] 7 | platforms = ["linux-64", "osx-64", "osx-arm64", "win-64"] 8 | license = "BSD-2-Clause" 9 | license-file = "LICENSE" 10 | preview = ["pixi-build"] 11 | 12 | [package] 13 | name = { workspace = true } 14 | version = { workspace = true } 15 | 16 | [package.build] 17 | backend = { name = "pixi-build-cmake", version = "0.3.*" } 18 | 19 | [package.build.config] 20 | extra-args = ["-DBUILD_TESTING=OFF"] 21 | 22 | [package.build-dependencies] 23 | pkg-config = ">=0.29.2" 24 | 25 | [package.host-dependencies] 26 | eigen = ">=3.4.0" 27 | pinocchio = ">=3.0.0" 28 | libboost-devel = ">=1.84.0" 29 | 30 | [package.run-dependencies] 31 | libboost-devel = ">=1.80.0" 32 | 33 | [dependencies] 34 | ccache = ">=4.9.1" 35 | cxx-compiler = ">=1.7.0" 36 | pkg-config = ">=0.29.2" 37 | cmake = ">=3.29.6" 38 | ninja = ">=1.12.1" 39 | eigen = ">=3.4.0" 40 | pinocchio = ">=3.0.0" 41 | libboost-devel = ">=1.84.0" 42 | example-robot-data = ">=4.1.0,<4.2" 43 | 44 | [tasks] 45 | configure = { cmd = [ 46 | "CXXFLAGS=$LOIK_CXX_FLAGS", 47 | "cmake", 48 | "-G", 49 | "Ninja", 50 | "-B", 51 | "build", 52 | "-S", 53 | ".", 54 | "-DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX", 55 | "-DCMAKE_BUILD_TYPE=$LOIK_BUILD_TYPE", 56 | ] } 57 | build = { cmd = "cmake --build build --target all", depends-on = ["configure"] } 58 | clean = { cmd = "rm -rf build" } 59 | test = { cmd = "ctest --test-dir build --output-on-failure", depends-on = [ 60 | "build", 61 | ] } 62 | 63 | [activation.env] 64 | # Setup ccache 65 | CMAKE_CXX_COMPILER_LAUNCHER = "ccache" 66 | # Create compile_commands.json for language server 67 | CMAKE_EXPORT_COMPILE_COMMANDS = "ON" 68 | # Activate color output with Ninja 69 | CMAKE_COLOR_DIAGNOSTICS = "ON" 70 | # Help ccache manage generated files and PCH (https://ccache.dev/manual/latest.html#_precompiled_headers) 71 | CCACHE_SLOPPINESS = "include_file_ctime,include_file_mtime,pch_defines,time_macros" 72 | 73 | [target.unix.activation] 74 | scripts = ["development/scripts/pixi/activation.sh"] 75 | 76 | [target.win-64.activation] 77 | scripts = ["development/scripts/pixi/activation.bat"] 78 | 79 | 80 | # Increment the version number with LOIK_VERSION variable 81 | [feature.new-version.dependencies] 82 | tomlkit = ">=0.13.2" 83 | 84 | [feature.new-version.tasks] 85 | configure-new-version = { cmd = [ 86 | "CXXFLAGS=$LOIK_CXX_FLAGS", 87 | "cmake", 88 | "-G", 89 | "Ninja", 90 | "-B", 91 | "build_new_version", 92 | "-S", 93 | ".", 94 | "-DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX", 95 | "-DCMAKE_BUILD_TYPE=$LOIK_BUILD_TYPE", 96 | ] } 97 | release-new-version = { cmd = "VERSION=$LOIK_VERSION cmake --build build_new_version --target release", depends-on = [ 98 | "configure-new-version", 99 | ] } 100 | 101 | [feature.lint] 102 | dependencies = { pre-commit = ">=3.6.2" } 103 | tasks = { lint = { cmd = "pre-commit run --all" } } 104 | 105 | # Use clang-cl on Windows 106 | # If something go wrong, we can check this repository: 107 | # https://github.com/conda-forge/clang-win-activation-feedstock/tree/main 108 | [feature.clang-cl] 109 | platforms = ["win-64"] 110 | dependencies = { clangxx = "*", lld = "*" } 111 | 112 | # Absolute path is needed to avoid using system clang-cl 113 | [feature.clang-cl.activation.env] 114 | CC = "%CONDA_PREFIX%\\Library\\bin\\clang-cl" 115 | CXX = "%CONDA_PREFIX%\\Library\\bin\\clang-cl" 116 | 117 | # Use clang on GNU/Linux 118 | [feature.clang] 119 | platforms = ["linux-64"] 120 | activation = { env = { CC = "clang", CXX = "clang++" } } 121 | dependencies = { clangxx = "*" } 122 | 123 | [feature.python-latest.dependencies] 124 | python = "3.14.*" 125 | 126 | [feature.python-oldest.dependencies] 127 | python = "3.10.*" 128 | 129 | [feature.test-pixi-build.dependencies] 130 | loik = { path = "." } 131 | cmake = ">=3.22" 132 | cxx-compiler = "*" 133 | cmake-package-check = "*" 134 | 135 | [feature.test-pixi-build.tasks] 136 | test-cmake = "cmake-package-check loik --targets loik::loik" 137 | test = { depends-on = ["test-cmake"] } 138 | 139 | [environments] 140 | default = { features = ["python-latest"], solve-group = "python-latest" } 141 | clang = { features = ["clang", "python-latest"] } 142 | lint = { features = ["lint"], solve-group = "python-latest" } 143 | python-oldest = { features = ["python-oldest"], solve-group = "python-oldest" } 144 | # Release a new software version 145 | new-version = { features = [ 146 | "new-version", 147 | "python-latest", 148 | ], solve-group = "python-latest" } 149 | clang-cl = { features = ["clang-cl", "python-latest"] } 150 | test-pixi-build = { features = ["test-pixi-build"], no-default-feature = true } 151 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LoIK 2 | 3 | **LoIK** is a simple yet efficient (constrained) differential inverse kinematics solver for robotics 4 | 5 | It is designed to function as an inner solver for various downstream applications, including global inverse kinematics and sampling-based motion planning. 6 | 7 | ## Features 8 | 9 | **LoIK** is a C++ template library, which provides: 10 | 11 | * a set of efficient solvers for constrained differential inverse kinematics problems 12 | * support for the [pinocchio](https://github.com/stack-of-tasks/pinocchio) rigid-body dynamics library 13 | * an interface to the [IKBench](https://github.com/Simple-Robotics/IKBench) inverse kinematics benchmark library which can be used to compare different IK solver performances 14 | * Python bindings leveraging [eigenpy](https://github.com/stack-of-tasks/eigenpy) {Next release} 15 | 16 | To cite **LoIK** in your publications, software, and research articles. 17 | Please refer to the [Citation section](#citing-loik) for further details. 18 | 19 | ## Installation 20 | 21 | 28 | 29 | 30 | ### Build from source 31 | 32 | ```bash 33 | git clone https://github.com/Simple-Robotics/LoIK --recursive 34 | cd LoIK 35 | mkdir build && cd build 36 | cmake .. -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=your_install_folder -DCMAKE_CXX_FLAGS="-march=native" 37 | make -jNCPUS 38 | make install 39 | ``` 40 | 41 | #### Dependencies 42 | * [Eigen3](https://eigen.tuxfamily.org) >= 3.4.0 43 | * [Boost](https://www.boost.org) >= 1.84.0 44 | * [Pinocchio](https://github.com/stack-of-tasks/pinocchio)>=3.0.0 | [conda](https://anaconda.org/conda-forge/pinocchio) 45 | * (optional) [eigenpy](https://github.com/stack-of-tasks/eigenpy)>=3.4.0 | [conda](https://anaconda.org/conda-forge/eigenpy) (Python bindings) 46 | * (optional) [example-robot-data](https://github.com/Gepetto/example-robot-data)>=4.1.0 | [conda](https://anaconda.org/conda-forge/example-robot-data) (required for examples and benchmarks) 47 | * a C++17 compliant compiler 48 | 49 | #### Notes 50 | 51 | * For developers, add the `-D CMAKE_EXPORT_COMPILE_COMMANDS=1` when working with language servers e.g. clangd. 52 | * To check for runtime Eigen memory allocation, add `-D CHECK_RUNTIME_MALLOC=ON` 53 | * By default, building the library will instantiate the templates for the `double` scalar type. 54 | * To build against a Conda environment, activate the environment and run `export CMAKE_PREFIX_PATH=$CONDA_PREFIX` before running CMake and use `$CONDA_PREFIX` as your install folder, i.e. add flag `-D CMAKE_INSTALL_PREFIX=$CONDA_PREFIX`. 55 | 56 | ### Build/install from source with Pixi 57 | 58 | To build **LoIK** from source the easiest way is to use [Pixi](https://pixi.sh/latest/#installation). 59 | 60 | [Pixi](https://pixi.sh/latest/) is a cross-platform package management tool for developers that 61 | will install all required dependencies in `.pixi` directory. 62 | It's used by our CI agent so you have the guarantee to get the right dependencies. 63 | 64 | Run the following command to install dependencies, configure, build and test the project: 65 | 66 | ```bash 67 | pixi run test 68 | ``` 69 | 70 | The project will be built in the `build` directory. 71 | You can now run `pixi shell` and build the project with `cmake` and `ninja` manually. 72 | 73 | 74 | ## Benchmarking 75 | 76 | We recommend [Flame Graphs](https://github.com/brendangregg/FlameGraph) for performance analysis. 77 | Please refer to this code analysis [tutorial](https://github.com/Simple-Robotics/code-analysis-tools?tab=readme-ov-file#install-1) for installation and usage of flame graph. 78 | 79 | ## Citing LoIK 80 | 81 | To cite **LoIK**, please use the following bibtex entry: 82 | 83 | ```bibtex 84 | @misc{loikapi, 85 | author = {Wingo, Bruce and Vaillant, Joris and Sathya, Ajay and Caron, Stéphane and Carpentier, Justin}, 86 | title = {LoIK}, 87 | url = {https://github.com/Simple-Robotics/LoIK} 88 | } 89 | ``` 90 | Please also consider citing the reference paper for the **LoIK** algorithm: 91 | 92 | ```bibtex 93 | @inproceedings{wingoLoIK2024, 94 | title = {{Linear-time Differential Inverse Kinematics: an Augmented Lagrangian Perspective}}, 95 | author = {Wingo, Bruce and Sathya, Ajay and Caron, Stéphane and Hutchinson, Seth and Carpentier, Justin}, 96 | year = {2024}, 97 | booktitle={Robotics: Science and Systems}, 98 | note = {https://inria.hal.science/hal-04607809v1} 99 | } 100 | ``` 101 | 102 | ## Contributors 103 | 104 | * [Bruce Wingo](https://bwingo47.github.io/) (Inria, Georgia Tech): main developer and manager of the project 105 | * [Ajay Sathya](https://scholar.google.com/citations?user=A00LDswAAAAJ&hl=en) (Inria): algorithm developer and core developer. 106 | * [Joris Vaillant](https://github.com/jorisv) (Inria): core developer 107 | * [Stéphane Caron](https://scaron.info/) (Inria): core developer 108 | * [Seth Hutchinson](https://faculty.cc.gatech.edu/~seth/) (Georgia Tech): project instructor 109 | * [Justin Carpentier](https://jcarpent.github.io/) (Inria): project instructor and core developer 110 | 111 | ## Acknowledgments 112 | 113 | The development of **LoIK** is actively supported by the [Willow team](https://www.di.ens.fr/willow/) at [@INRIA](http://www.inria.fr) Paris. 114 | -------------------------------------------------------------------------------- /include/loik/task-solver-base.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | namespace loik 11 | { 12 | 13 | enum ADMMPenaltyUpdateStrat 14 | { 15 | DEFAULT = 0, 16 | OSQP = 1, 17 | MAXEIGENVALUE = 3 18 | }; // enum ADMMPenaltyUpdateStrat 19 | 20 | template 21 | struct IkIdSolverBaseTpl 22 | { 23 | typedef _Scalar Scalar; 24 | 25 | struct SolverInfo 26 | { 27 | explicit SolverInfo(const int max_iter) 28 | { 29 | primal_residual_list_.reserve(static_cast(max_iter)); 30 | dual_residual_list_.reserve(static_cast(max_iter)); 31 | mu_list_.reserve(static_cast(max_iter)); 32 | iter_list_.reserve(static_cast(max_iter)); 33 | }; 34 | 35 | void Reset() 36 | { 37 | primal_residual_list_.clear(); 38 | dual_residual_list_.clear(); 39 | mu_list_.clear(); 40 | iter_list_.clear(); 41 | }; 42 | 43 | int Size() const 44 | { 45 | return iter_list_.size(); 46 | } 47 | 48 | std::vector primal_residual_list_; 49 | std::vector dual_residual_list_; 50 | std::vector mu_list_; 51 | std::vector iter_list_; 52 | }; 53 | 54 | IkIdSolverBaseTpl( 55 | const int max_iter, 56 | const Scalar & tol_abs, 57 | const Scalar & tol_rel, 58 | const Scalar & tol_primal_inf, 59 | const Scalar & tol_dual_inf, 60 | const Scalar & rho, 61 | const Scalar & mu, 62 | const Scalar & mu_equality_scale_factor, 63 | const ADMMPenaltyUpdateStrat & mu_update_strat, 64 | const bool verbose, 65 | const bool logging) 66 | : rho_(rho) 67 | , mu0_(mu) 68 | , mu_(mu) 69 | , mu_equality_scale_factor_(mu_equality_scale_factor) 70 | , mu_update_strat_(mu_update_strat) 71 | , max_iter_(max_iter) 72 | , tol_abs_(tol_abs) 73 | , tol_rel_(tol_rel) 74 | , tol_primal_inf_(tol_primal_inf) 75 | , tol_dual_inf_(tol_dual_inf) 76 | , verbose_(verbose) 77 | , logging_(logging) 78 | , solver_info_(max_iter) {}; 79 | 80 | /// \brief reset solver 81 | void Reset() 82 | { 83 | 84 | iter_ = 0; 85 | converged_ = false; 86 | primal_infeasible_ = false; 87 | dual_infeasible_ = false; 88 | 89 | mu_ = mu0_; 90 | }; 91 | 92 | /// \brief get current iteration count 93 | inline int get_iter() const 94 | { 95 | return iter_; 96 | }; 97 | 98 | /// \brief get primal residual at the last finished iteration 99 | inline Scalar get_primal_residual() const 100 | { 101 | return primal_residual_; 102 | }; 103 | 104 | /// \brief get dual residual at the last finished iteration 105 | inline Scalar get_dual_residual() const 106 | { 107 | return dual_residual_; 108 | }; 109 | 110 | /// \brief get convergence status at the last finished iteration 111 | inline bool get_convergence_status() const 112 | { 113 | return converged_; 114 | }; 115 | 116 | /// \brief get primal infeasibility status at the last finished iteration 117 | inline bool get_primal_infeasibility_status() const 118 | { 119 | return primal_infeasible_; 120 | }; 121 | 122 | /// \brief get dual infeasibility status at the last finished iteration 123 | inline bool get_dual_infeasibility_status() const 124 | { 125 | return dual_infeasible_; 126 | }; 127 | 128 | /// \brief set the number of maximum iterations 129 | inline void set_max_iter(const int max_iter) 130 | { 131 | this->max_iter_ = max_iter; 132 | }; 133 | 134 | /// \brief set proximal parameter 135 | inline void set_rho(const Scalar rho) 136 | { 137 | this->rho_ = rho; 138 | }; 139 | 140 | /// \brief get proximal parameter 141 | inline Scalar get_rho() const 142 | { 143 | return rho_; 144 | }; 145 | 146 | /// \brief set ADMM master penalty 147 | inline void set_mu(const Scalar mu) 148 | { 149 | this->mu_ = mu; 150 | }; 151 | 152 | /// \brief get ADMM master penalty 153 | inline Scalar get_mu() const 154 | { 155 | return mu_; 156 | }; 157 | 158 | /// \brief set primal convergence tolerance directly 159 | inline void set_tol_primal(const Scalar tol_primal) 160 | { 161 | this->tol_primal_ = tol_primal; 162 | }; 163 | 164 | /// \brief get primal convergence tolerance 165 | inline Scalar get_tol_primal() const 166 | { 167 | return tol_primal_; 168 | }; 169 | 170 | /// \brief set dual convergence tolerance directly 171 | inline void set_tol_dual(const Scalar tol_dual) 172 | { 173 | this->tol_dual_ = tol_dual; 174 | }; 175 | 176 | /// \brief get dual convergence tolerance 177 | inline Scalar get_tol_dual() const 178 | { 179 | return tol_dual_; 180 | }; 181 | 182 | /// \brief set primal infeasibility tolerance directly 183 | inline void set_tol_primal_inf(const Scalar tol_primal_inf) 184 | { 185 | this->tol_primal_inf_ = tol_primal_inf; 186 | }; 187 | 188 | /// \brief get primal infeasibility tolerance 189 | inline Scalar get_tol_primal_inf() const 190 | { 191 | return tol_primal_inf_; 192 | }; 193 | 194 | /// \brief set dual infeasibility tolerance directly 195 | inline void set_tol_dual_inf(const Scalar tol_dual_inf) 196 | { 197 | this->tol_dual_inf_ = tol_dual_inf; 198 | }; 199 | 200 | /// \brief get dual infeasibility tolerance 201 | inline Scalar get_tol_dual_inf() const 202 | { 203 | return tol_dual_inf_; 204 | }; 205 | 206 | protected: 207 | Scalar rho_; // proximal parameter for primal vars solved in the equality constraint QP step 208 | Scalar mu0_; // initial ADMM master penalty parameter 209 | Scalar mu_; // ADMM master penalty parameter 210 | Scalar mu_equality_scale_factor_; // ADMM penalty scaling factor for equality constraints 211 | 212 | ADMMPenaltyUpdateStrat mu_update_strat_; // enum to switch between mu update strategies 213 | 214 | int max_iter_; // solver maximum iteration 215 | int iter_; // current iteration 216 | bool converged_; // convergence flag 217 | Scalar tol_abs_; // absolute tol 218 | Scalar tol_rel_; // relative tol 219 | Scalar tol_primal_; // primal residual convergence tol 220 | Scalar tol_dual_; // dual residual convergence tol 221 | Scalar tol_primal_inf_; // primal infeasibility tol 222 | Scalar tol_dual_inf_; // dual infeasibility tol 223 | bool primal_infeasible_; // primal infeasibility flag 224 | bool dual_infeasible_; // dual infeasibility flag 225 | 226 | Scalar primal_residual_; // primal residual at current iter 227 | Scalar dual_residual_; // dual residual at current iter 228 | 229 | bool verbose_; // verbose printing 230 | bool logging_; // solver data logging 231 | SolverInfo solver_info_; // solver logging info 232 | 233 | }; // struct IkIdSolverBaseTpl 234 | 235 | } // namespace loik 236 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (C) 2024 INRIA 3 | # 4 | 5 | cmake_minimum_required(VERSION 3.22) 6 | 7 | set(PROJECT_NAME loik) 8 | set(PROJECT_DESCRIPTION "Low-Order Inverse Kinematics") 9 | set(PROJECT_URL "https://github.com/Simple-Robotics/LoIK") 10 | set(PROJECT_CUSTOM_HEADER_EXTENSION "hpp") 11 | set(PROJECT_CUSTOM_HEADER_DIR "loik") 12 | set(PROJECT_USE_KEYWORD_LINK_LIBRARIES True) 13 | # To enable jrl-cmakemodules compatibility with workspace we must define the two following lines 14 | set(PROJECT_AUTO_RUN_FINALIZE FALSE) 15 | set(PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}) 16 | 17 | set(CXX_DISABLE_WERROR True) 18 | 19 | set(CMAKE_CXX_STANDARD 17) 20 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 21 | set(CMAKE_CXX_EXTENSIONS OFF) 22 | set(PROJECT_USE_CMAKE_EXPORT True) 23 | 24 | option(INSTALL_DOCUMENTATION "Generate and install the documentation" ON) 25 | 26 | # Check if the submodule cmake have been initialized 27 | set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") 28 | if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake") 29 | message(STATUS "JRL cmakemodules found in 'cmake/' git submodule") 30 | else() 31 | find_package(jrl-cmakemodules QUIET CONFIG) 32 | if(jrl-cmakemodules_FOUND) 33 | get_property( 34 | JRL_CMAKE_MODULES 35 | TARGET jrl-cmakemodules::jrl-cmakemodules 36 | PROPERTY INTERFACE_INCLUDE_DIRECTORIES) 37 | message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}") 38 | elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0") 39 | message( 40 | FATAL_ERROR 41 | "\nCan't find jrl-cmakemodules. Please either:\n" 42 | " - use git submodule: 'git submodule update --init'\n" 43 | " - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n" 44 | " - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n") 45 | else() 46 | message(STATUS "JRL cmakemodules not found. Let's fetch it.") 47 | include(FetchContent) 48 | FetchContent_Declare("jrl-cmakemodules" 49 | GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git") 50 | FetchContent_MakeAvailable("jrl-cmakemodules") 51 | FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES) 52 | endif() 53 | endif() 54 | 55 | set(DOXYGEN_USE_MATHJAX YES) 56 | set(DOXYGEN_USE_TEMPLATE_CSS YES) 57 | 58 | # Use BoostConfig module distributed by boost library instead of using FindBoost module distributed 59 | # by CMake (to remove in 3.30). This policy is not working when using clang-cl. 60 | if(NOT WIN32 OR NOT ${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") 61 | if(POLICY CMP0167) 62 | cmake_policy(SET CMP0167 NEW) 63 | set(CMAKE_POLICY_DEFAULT_CMP0167 NEW) 64 | endif() 65 | endif() 66 | include(${JRL_CMAKE_MODULES}/base.cmake) 67 | compute_project_args(PROJECT_ARGS LANGUAGES CXX) 68 | project(${PROJECT_NAME} ${PROJECT_ARGS}) 69 | set(CMAKE_VERBOSE_MAKEFILE TRUE) 70 | 71 | include(${JRL_CMAKE_MODULES}/boost.cmake) 72 | include(${JRL_CMAKE_MODULES}/ide.cmake) 73 | include(${JRL_CMAKE_MODULES}/apple.cmake) 74 | include(CMakeDependentOption) 75 | 76 | apply_default_apple_configuration() 77 | 78 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 79 | message(STATUS "Setting build type to 'Release' as none was specified.") 80 | set(CMAKE_BUILD_TYPE 81 | Release 82 | CACHE STRING "Choose the type of build." FORCE) 83 | # Set the possible values of build type for cmake-gui 84 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" 85 | "RelWithDebInfo") 86 | endif() 87 | 88 | # --- OPTIONS ---------------------------------------- 89 | option(BUILD_WITH_VERSION_SUFFIX "Build libraries with version appended to suffix" OFF) 90 | option(ENABLE_TEMPLATE_INSTANTIATION "Template instantiation of the main library" ON) 91 | 92 | # --- OPTIONAL DEPENDENCIES ------------------------- 93 | option(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF) 94 | option(CHECK_RUNTIME_MALLOC "Check if some memory allocations are performed at runtime" OFF) 95 | 96 | # Variable containing all the cflags definition relative to optional dependencies and options 97 | set(CFLAGS_DEPENDENCIES) 98 | 99 | if(INITIALIZE_WITH_NAN) 100 | message(STATUS "Initialize with NaN all the Eigen entries.") 101 | add_compile_definitions(EIGEN_INITIALIZE_MATRICES_BY_NAN) 102 | endif() 103 | 104 | if(CHECK_RUNTIME_MALLOC) 105 | message(STATUS "Check if some memory allocations are performed at runtime.") 106 | add_compile_definitions(LOIK_EIGEN_CHECK_MALLOC) 107 | add_compile_definitions(EIGEN_RUNTIME_NO_MALLOC) 108 | endif() 109 | 110 | if(ENABLE_TEMPLATE_INSTANTIATION) 111 | add_compile_definitions(LOIK_ENABLE_TEMPLATE_INSTANTIATION) 112 | list(APPEND CFLAGS_DEPENDENCIES "-DLOIK_ENABLE_TEMPLATE_INSTANTIATION") 113 | endif() 114 | 115 | macro(TAG_LIBRARY_VERSION target) 116 | set_target_properties(${target} PROPERTIES SOVERSION ${PROJECT_VERSION}) 117 | endmacro() 118 | 119 | # ---------------------------------------------------- 120 | # --- DEPENDENCIES ----------------------------------- 121 | # ---------------------------------------------------- 122 | add_project_dependency(Eigen3 3.4.0 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.4.0") 123 | add_project_dependency(pinocchio REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 3.0.0") 124 | 125 | set(BOOST_REQUIRED_COMPONENTS serialization) 126 | set_boost_default_options() 127 | export_boost_default_options() 128 | add_project_dependency(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS}) 129 | 130 | # --- MAIN LIBRARY ---------------------------------------- 131 | set(LIB_HEADER_DIR ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}) 132 | set(LIB_HEADERS 133 | ${LIB_HEADER_DIR}/fwd.hpp 134 | ${LIB_HEADER_DIR}/ik-id-description.hpp 135 | ${LIB_HEADER_DIR}/ik-id-description-optimized.hpp 136 | ${LIB_HEADER_DIR}/loik-loid-data.hpp 137 | ${LIB_HEADER_DIR}/loik-loid-data.hxx 138 | ${LIB_HEADER_DIR}/loik-loid-data-optimized.hpp 139 | ${LIB_HEADER_DIR}/loik-loid-data-optimized.hxx 140 | ${LIB_HEADER_DIR}/loik-loid.hpp 141 | ${LIB_HEADER_DIR}/loik-loid.hxx 142 | ${LIB_HEADER_DIR}/loik-loid-optimized.hpp 143 | ${LIB_HEADER_DIR}/loik-loid-optimized.hxx 144 | ${LIB_HEADER_DIR}/task-solver-base.hpp) 145 | set(LIB_TEMPLATE_INSTANTIATION_HEADERS 146 | ${LIB_HEADER_DIR}/loik-loid.txx ${LIB_HEADER_DIR}/loik-loid-optimized.txx 147 | ${LIB_HEADER_DIR}/loik-loid-data.txx ${LIB_HEADER_DIR}/loik-loid-data-optimized.txx) 148 | 149 | set(LIB_SOURCE_DIR ${PROJECT_SOURCE_DIR}/src) 150 | set(LIB_SOURCES) 151 | set(LIB_TEMPLATE_INSTANTIATION_SOURCES 152 | ${LIB_SOURCE_DIR}/loik-loid.cpp ${LIB_SOURCE_DIR}/loik-loid-optimized.cpp 153 | ${LIB_SOURCE_DIR}/loik-loid-data.cpp ${LIB_SOURCE_DIR}/loik-loid-data-optimized.cpp) 154 | 155 | if(ENABLE_TEMPLATE_INSTANTIATION) 156 | list(APPEND LIB_HEADERS ${LIB_TEMPLATE_INSTANTIATION_HEADERS}) 157 | list(APPEND LIB_SOURCES ${LIB_TEMPLATE_INSTANTIATION_SOURCES}) 158 | endif() 159 | 160 | function(set_standard_output_directory target) 161 | set_target_properties( 162 | ${target} 163 | PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin 164 | LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib 165 | ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib) 166 | endfunction() 167 | 168 | function(create_library) 169 | # get the compile defs 170 | get_directory_property(CURRENT_COMPILE_DEFINITIONS COMPILE_DEFINITIONS) 171 | 172 | set(library_type INTERFACE) 173 | if(LIB_SOURCES) 174 | set(library_type SHARED) 175 | endif() 176 | 177 | add_library(${PROJECT_NAME} ${library_type} ${LIB_HEADERS} ${LIB_SOURCES}) 178 | add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) 179 | set_standard_output_directory(${PROJECT_NAME}) 180 | set_target_properties( 181 | ${PROJECT_NAME} 182 | PROPERTIES LINKER_LANGUAGE CXX 183 | INSTALL_RPATH "\$ORIGIN" 184 | VERSION ${PROJECT_VERSION}) 185 | 186 | cxx_flags_by_compiler_frontend(MSVC "NOMINMAX" OUTPUT PUBLIC_DEFINITIONS) 187 | target_compile_definitions(${PROJECT_NAME} PUBLIC ${CURRENT_COMPILE_DEFINITIONS} 188 | ${PUBLIC_DEFINITIONS}) 189 | 190 | if(BUILD_LIBRARY_WITH_VERSION_SUFFIX) 191 | tag_library_version(${PROJECT_NAME}) 192 | endif() 193 | 194 | target_link_libraries(${PROJECT_NAME} PUBLIC pinocchio::pinocchio) 195 | target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen) 196 | target_link_libraries(${PROJECT_NAME} PUBLIC Boost::boost) 197 | 198 | target_include_directories( 199 | ${PROJECT_NAME} 200 | PUBLIC $ $ 201 | $) 202 | 203 | cxx_flags_by_compiler_frontend( 204 | MSVC "/bigobj" 205 | OUTPUT PUBLIC_OPTIONS 206 | FILTER) 207 | target_compile_options(${PROJECT_NAME} PUBLIC ${PUBLIC_OPTIONS}) 208 | endfunction() 209 | 210 | create_library() 211 | 212 | add_header_group(LIB_HEADERS) 213 | add_source_group(LIB_SOURCES) 214 | # Allow jrl-cmakemodules to install headers in the right directory 215 | set(${PROJECT_NAME}_HEADERS ${LIB_HEADERS}) 216 | 217 | install( 218 | TARGETS ${PROJECT_NAME} 219 | EXPORT ${TARGETS_EXPORT_NAME} 220 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 221 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 222 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 223 | 224 | if(BUILD_TESTING) 225 | add_subdirectory(tests) 226 | endif() 227 | 228 | # --- PACKAGING ---------------------------------------------------------------- 229 | macro(EXPORT_VARIABLE var_name var_value) 230 | get_directory_property(has_parent PARENT_DIRECTORY) 231 | if(has_parent) 232 | set(${var_name} 233 | ${var_value} 234 | PARENT_SCOPE) 235 | else() 236 | set(${var_name} ${var_value}) 237 | endif() 238 | string(APPEND PACKAGE_EXTRA_MACROS "\nset(${var_name} ${var_value})") 239 | endmacro() 240 | 241 | if(ENABLE_TEMPLATE_INSTANTIATION) 242 | export_variable(LOIK_ENABLE_TEMPLATE_INSTANTIATION ON) 243 | endif() 244 | 245 | pkg_config_append_libs(${PROJECT_NAME}) 246 | pkg_config_append_boost_libs(${BOOST_REQUIRED_COMPONENTS}) 247 | pkg_config_append_cflags("${CFLAGS_DEPENDENCIES}") 248 | 249 | setup_project_finalize() 250 | -------------------------------------------------------------------------------- /include/loik/loik-loid-data.hxx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid-data.hpp" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace loik 17 | { 18 | 19 | /////////////////////////////////////////////////////////////////////////////////////////////// 20 | /////////////////////////////////////////////////////////////////////////////////////////////// 21 | /////////////////////////////////////////////////////////////////////////////////////////////// 22 | /// 23 | /// \brief IkIdDataTpl constructor with default constraint dimension of 6 24 | /// 25 | template class JointCollectionTpl> 26 | IkIdDataTpl<_Scalar, _Options, JointCollectionTpl>::IkIdDataTpl(const Model & model) 27 | : IkIdDataTpl(model, 6) // delegating constructor call 28 | { 29 | // 30 | } 31 | 32 | /////////////////////////////////////////////////////////////////////////////////////////////// 33 | /////////////////////////////////////////////////////////////////////////////////////////////// 34 | /////////////////////////////////////////////////////////////////////////////////////////////// 35 | /// 36 | /// \brief IkIdDataTpl constructor 37 | /// 38 | template class JointCollectionTpl> 39 | IkIdDataTpl<_Scalar, _Options, JointCollectionTpl>::IkIdDataTpl( 40 | const Model & model, const int eq_c_dim) 41 | : joints(0) 42 | , oMi(static_cast(model.njoints), SE3::Identity()) 43 | , liMi(static_cast(model.njoints), SE3::Identity()) 44 | , nu(DVec::Zero(model.nv)) 45 | , nu_prev(DVec::Zero(model.nv)) 46 | , vis(static_cast(model.njoints), Motion::Zero()) 47 | , Si_nui_s(static_cast(model.njoints), Motion::Zero()) 48 | , ovis(static_cast(model.njoints), Motion::Zero()) 49 | , vis_prev(static_cast(model.njoints), Motion::Zero()) 50 | , His_aba(static_cast(model.njoints), Mat6x6::Identity()) 51 | , His(static_cast(model.njoints), Mat6x6::Identity()) 52 | , Pis(static_cast(model.njoints), Mat6x6::Identity()) 53 | , pis(static_cast(model.njoints), Vec6::Zero()) 54 | , Dis(static_cast(model.njoints)) 55 | , Di_invs(static_cast(model.njoints)) 56 | , Ris(static_cast(model.njoints)) 57 | , ris(static_cast(model.njoints)) 58 | , fis(static_cast(model.njoints), Vec6::Zero()) 59 | , yis(static_cast(model.njoints), DVec::Zero(eq_c_dim)) 60 | , yis_prev(static_cast(model.njoints), DVec::Zero(eq_c_dim)) 61 | , w(DVec::Zero(model.nv)) 62 | , w_prev(DVec::Zero(model.nv)) 63 | , z(DVec::Zero(model.nv)) 64 | , z_prev(DVec::Zero(model.nv)) 65 | , eq_c_dim(eq_c_dim) 66 | { 67 | /* Create data structure associated to the joints */ 68 | for (JointIndex i = 0; i < static_cast(model.njoints); ++i) 69 | { 70 | // create joint datas 71 | joints.push_back( 72 | pinocchio::CreateJointData<_Scalar, _Options, JointCollectionTpl>::run(model.joints[i])); 73 | 74 | // fill Dis, Di_invs, Ris, and ris 75 | Ris[i] = DMat::Zero(model.joints[i].nv(), model.joints[i].nv()); 76 | ris[i] = DVec::Zero(model.joints[i].nv()); 77 | Dis[i] = DMat::Zero(model.joints[i].nv(), model.joints[i].nv()); 78 | Di_invs[i] = DMat::Zero(model.joints[i].nv(), model.joints[i].nv()); 79 | } 80 | 81 | // initialize utility members 82 | joint_full_range.reserve(static_cast(model.njoints)); 83 | joint_full_range.resize(static_cast(model.njoints)); 84 | joint_range.reserve(static_cast(model.njoints) - 1); 85 | joint_range.resize(static_cast(model.njoints) - 1); 86 | std::iota(joint_full_range.begin(), joint_full_range.end(), 0); // [0, nb_ - 1] 87 | std::iota(joint_range.begin(), joint_range.end(), 1); // [1, nb_ - 1] 88 | } 89 | 90 | /////////////////////////////////////////////////////////////////////////////////////////////// 91 | /////////////////////////////////////////////////////////////////////////////////////////////// 92 | /////////////////////////////////////////////////////////////////////////////////////////////// 93 | /// 94 | /// \brief Reset all quantites 95 | /// 96 | template class JointCollectionTpl> 97 | void IkIdDataTpl<_Scalar, _Options, JointCollectionTpl>::Reset(const bool warm_start) 98 | { 99 | 100 | if (!warm_start) 101 | { 102 | // no warm start, wipe everything 103 | nu.setZero(); 104 | nu_prev.setZero(); 105 | w.setZero(); 106 | w_prev.setZero(); 107 | z.setZero(); 108 | z_prev.setZero(); 109 | 110 | for (auto & idx : joint_full_range) 111 | { 112 | oMi[idx].setIdentity(); 113 | liMi[idx].setIdentity(); 114 | 115 | vis[idx].setZero(); 116 | Si_nui_s[idx].setZero(); 117 | ovis[idx].setZero(); 118 | vis_prev[idx].setZero(); 119 | 120 | His_aba[idx].setZero(); 121 | His[idx].setZero(); 122 | Pis[idx].setZero(); 123 | pis[idx].setZero(); 124 | Dis[idx].setZero(); 125 | Di_invs[idx].setZero(); 126 | Ris[idx].setZero(); 127 | ris[idx].setZero(); 128 | fis[idx].setZero(); 129 | yis[idx].setZero(); 130 | yis_prev[idx].setZero(); 131 | } 132 | } 133 | else 134 | { 135 | // warm start, keep primal and dual variables from previous solve() call 136 | nu_prev = nu; 137 | w_prev = w; 138 | z_prev = z; 139 | 140 | for (auto & idx : joint_full_range) 141 | { 142 | oMi[idx].setIdentity(); 143 | liMi[idx].setIdentity(); 144 | 145 | // wipe recursion related quantites 146 | His_aba[idx].setZero(); 147 | His[idx].setZero(); 148 | Pis[idx].setZero(); 149 | pis[idx].setZero(); 150 | Dis[idx].setZero(); 151 | Di_invs[idx].setZero(); 152 | Ris[idx].setZero(); 153 | ris[idx].setZero(); 154 | fis[idx].setZero(); 155 | 156 | // update _prev variables without wiping current primal dual variables 157 | vis_prev[idx] = vis[idx]; 158 | yis_prev[idx] = yis[idx]; 159 | } 160 | } 161 | 162 | } // Reset 163 | 164 | /////////////////////////////////////////////////////////////////////////////////////////////// 165 | /////////////////////////////////////////////////////////////////////////////////////////////// 166 | /////////////////////////////////////////////////////////////////////////////////////////////// 167 | /// 168 | /// \brief Reset all recursion related quantites 169 | /// 170 | template class JointCollectionTpl> 171 | void IkIdDataTpl<_Scalar, _Options, JointCollectionTpl>::ResetRecursion() 172 | { 173 | 174 | // no warm start, wipe everything 175 | nu.setZero(); 176 | nu_prev.setZero(); 177 | w.setZero(); 178 | w_prev.setZero(); 179 | z.setZero(); 180 | z_prev.setZero(); 181 | 182 | for (auto & idx : joint_full_range) 183 | { 184 | vis[idx].setZero(); 185 | Si_nui_s[idx].setZero(); 186 | ovis[idx].setZero(); 187 | vis_prev[idx].setZero(); 188 | 189 | His_aba[idx].setZero(); 190 | His[idx].setZero(); 191 | Pis[idx].setZero(); 192 | pis[idx].setZero(); 193 | Dis[idx].setZero(); 194 | Di_invs[idx].setZero(); 195 | Ris[idx].setZero(); 196 | ris[idx].setZero(); 197 | fis[idx].setZero(); 198 | yis[idx].setZero(); 199 | yis_prev[idx].setZero(); 200 | } 201 | 202 | } // ResetRecursion 203 | 204 | /////////////////////////////////////////////////////////////////////////////////////////////// 205 | /////////////////////////////////////////////////////////////////////////////////////////////// 206 | /////////////////////////////////////////////////////////////////////////////////////////////// 207 | /// 208 | /// \brief update _prev primal dual variables with that of current values 209 | /// 210 | template class JointCollectionTpl> 211 | void IkIdDataTpl<_Scalar, _Options, JointCollectionTpl>::UpdatePrev() 212 | { 213 | nu_prev = nu; 214 | for (auto & idx : joint_full_range) 215 | { 216 | vis_prev[idx] = vis[idx]; 217 | yis_prev[idx] = yis[idx]; 218 | } 219 | w_prev = w; 220 | z_prev = z; 221 | } 222 | 223 | /////////////////////////////////////////////////////////////////////////////////////////////// 224 | /////////////////////////////////////////////////////////////////////////////////////////////// 225 | /////////////////////////////////////////////////////////////////////////////////////////////// 226 | /// 227 | /// \brief operator "==" overload, as a Non-member function for symmetric behavior, i.e. a == b 228 | /// and b == a shouldm give the same result. 229 | /// 230 | template class JointCollectionTpl> 231 | bool operator==( 232 | const IkIdDataTpl & ik_id_data_1, 233 | const IkIdDataTpl & ik_id_data_2) 234 | { 235 | bool value = 236 | ik_id_data_1.joints == ik_id_data_2.joints && ik_id_data_1.oMi == ik_id_data_2.oMi 237 | && ik_id_data_1.liMi == ik_id_data_2.liMi && ik_id_data_1.nu == ik_id_data_2.nu 238 | && ik_id_data_1.nu_prev == ik_id_data_2.nu_prev && ik_id_data_1.vis == ik_id_data_2.vis 239 | && ik_id_data_1.Si_nui_s == ik_id_data_2.Si_nui_s && ik_id_data_1.ovis == ik_id_data_2.ovis 240 | && ik_id_data_1.vis_prev == ik_id_data_2.vis_prev 241 | && ik_id_data_1.His_aba == ik_id_data_2.His_aba && ik_id_data_1.His == ik_id_data_2.His 242 | && ik_id_data_1.Pis == ik_id_data_2.Pis && ik_id_data_1.pis == ik_id_data_2.pis 243 | && ik_id_data_1.Dis == ik_id_data_2.Dis && ik_id_data_1.Di_invs == ik_id_data_2.Di_invs 244 | && ik_id_data_1.Ris == ik_id_data_2.Ris && ik_id_data_1.ris == ik_id_data_2.ris 245 | && ik_id_data_1.fis == ik_id_data_2.fis && ik_id_data_1.yis == ik_id_data_2.yis 246 | && ik_id_data_1.yis_prev == ik_id_data_2.yis_prev && ik_id_data_1.w == ik_id_data_2.w 247 | && ik_id_data_1.w_prev == ik_id_data_2.w_prev && ik_id_data_1.z == ik_id_data_2.z 248 | && ik_id_data_1.z_prev == ik_id_data_2.z_prev 249 | && ik_id_data_1.joint_full_range == ik_id_data_2.joint_full_range 250 | && ik_id_data_1.joint_range == ik_id_data_2.joint_range; 251 | 252 | return value; 253 | } 254 | 255 | /////////////////////////////////////////////////////////////////////////////////////////////// 256 | /////////////////////////////////////////////////////////////////////////////////////////////// 257 | /////////////////////////////////////////////////////////////////////////////////////////////// 258 | /// 259 | /// \brief operator "!=" overload, as a Non-member function for symmetric behavior, i.e. a != b 260 | /// and b != a shouldm give the same result. 261 | /// 262 | template class JointCollectionTpl> 263 | bool operator!=( 264 | const IkIdDataTpl & ik_id_data_1, 265 | const IkIdDataTpl & ik_id_data_2) 266 | { 267 | return !(ik_id_data_1 == ik_id_data_2); 268 | } 269 | 270 | /////////////////////////////////////////////////////////////////////////////////////////////// 271 | /////////////////////////////////////////////////////////////////////////////////////////////// 272 | /////////////////////////////////////////////////////////////////////////////////////////////// 273 | /// 274 | /// \brief utility function for defining symmetric operator "==" overload between IkIdData and 275 | /// Data 276 | /// 277 | template class JointCollectionTpl> 278 | bool CompareDataIkIdData( 279 | const IkIdDataTpl & ik_id_data, 280 | const pinocchio::DataTpl & data) 281 | { 282 | bool value = ik_id_data.oMi == data.oMi && ik_id_data.liMi == data.liMi; 283 | 284 | // check joint data created btw "ik_id_data" and "data" are the same 285 | for (std::size_t j = 1; j < static_cast(ik_id_data.joints.size()); ++j) 286 | { 287 | value &= ik_id_data.joints.at(j) == data.joints.at(j); 288 | } 289 | 290 | return value; 291 | } 292 | 293 | /////////////////////////////////////////////////////////////////////////////////////////////// 294 | /////////////////////////////////////////////////////////////////////////////////////////////// 295 | /////////////////////////////////////////////////////////////////////////////////////////////// 296 | /// 297 | /// \brief symmetric operator "==" overloads between IkIdData and Data 298 | /// 299 | template class JointCollectionTpl> 300 | bool operator==( 301 | const IkIdDataTpl & ik_id_data, 302 | const pinocchio::DataTpl & data) 303 | { 304 | return CompareDataIkIdData(ik_id_data, data); 305 | } 306 | 307 | template class JointCollectionTpl> 308 | bool operator==( 309 | const pinocchio::DataTpl & data, 310 | const IkIdDataTpl & ik_id_data) 311 | { 312 | return CompareDataIkIdData(ik_id_data, data); 313 | } 314 | 315 | /////////////////////////////////////////////////////////////////////////////////////////////// 316 | /////////////////////////////////////////////////////////////////////////////////////////////// 317 | /////////////////////////////////////////////////////////////////////////////////////////////// 318 | /// 319 | /// \brief symmetric operator "!=" overloads between IkIdData and Data 320 | /// 321 | template class JointCollectionTpl> 322 | bool operator!=( 323 | const IkIdDataTpl & ik_id_data, 324 | const pinocchio::DataTpl & data) 325 | { 326 | return !CompareDataIkIdData(ik_id_data, data); 327 | } 328 | 329 | template class JointCollectionTpl> 330 | bool operator!=( 331 | const pinocchio::DataTpl & data, 332 | const IkIdDataTpl & ik_id_data) 333 | { 334 | return !CompareDataIkIdData(ik_id_data, data); 335 | } 336 | 337 | } // namespace loik 338 | -------------------------------------------------------------------------------- /include/loik/loik-loid-data.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "fwd.hpp" 8 | #include "loik/config.hpp" 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | // namespace loik 35 | // { 36 | 37 | // template< 38 | // typename _Scalar, 39 | // int _Options = 0, 40 | // template class JointCollectionTpl = pinocchio::JointCollectionDefaultTpl> 41 | // struct IkIdDataTpl; 42 | 43 | // } 44 | 45 | namespace pinocchio 46 | { 47 | // trait specialization for pinocchio::ik_id::traits 48 | template class JointCollectionTpl> 49 | struct traits> 50 | { 51 | typedef _Scalar Scalar; 52 | }; 53 | } // namespace pinocchio 54 | 55 | namespace loik 56 | { 57 | 58 | // IkIdDataTpl definition 59 | template class JointCollectionTpl> 60 | struct IkIdDataTpl 61 | : pinocchio::serialization::Serializable> 62 | , pinocchio::NumericalBase> 63 | { 64 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 65 | 66 | typedef _Scalar Scalar; 67 | enum 68 | { 69 | Options = _Options 70 | }; 71 | 72 | typedef pinocchio::ModelTpl Model; 73 | 74 | typedef pinocchio::SE3Tpl SE3; 75 | typedef pinocchio::MotionTpl Motion; 76 | typedef pinocchio::ForceTpl Force; 77 | typedef pinocchio::InertiaTpl Inertia; 78 | typedef pinocchio::FrameTpl Frame; 79 | 80 | typedef pinocchio::Index Index; 81 | typedef pinocchio::JointIndex JointIndex; 82 | typedef pinocchio::FrameIndex FrameIndex; 83 | typedef std::vector IndexVector; 84 | 85 | typedef pinocchio::JointModelTpl JointModel; 86 | typedef pinocchio::JointDataTpl JointData; 87 | 88 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; 89 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; 90 | 91 | typedef Eigen::Matrix DMat; 92 | typedef Eigen::Matrix DVec; 93 | typedef Eigen::Matrix Vec3; 94 | typedef Eigen::Matrix Vec6; 95 | typedef Eigen::Matrix Mat6x6; 96 | 97 | /// \brief Dense vectorized version of a joint configuration vector, q. 98 | typedef DVec ConfigVectorType; 99 | 100 | /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, 101 | /// acceleration, etc), q_dot. 102 | /// It also handles the notion of co-tangent vector (e.g. torque, etc). 103 | typedef DVec TangentVectorType; 104 | 105 | /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in 106 | /// model, encapsulated in JointDataAccessor. 107 | JointDataVector joints; 108 | 109 | /// \brief Vector of absolute joint placements (wrt the world). 110 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi; 111 | 112 | /// \brief Vector of relative joint placements (wrt the body parent). 113 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi; 114 | 115 | /// \brief generalized velocity 116 | DVec nu; 117 | 118 | /// \brief generalized velocity from the previous solver iteration 119 | DVec nu_prev; 120 | 121 | /// \brief link spatial velocity expressed in link's local frame 122 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) vis; 123 | 124 | /// \brief temporary variable that stores the link's spatial velocity caused by it's parent 125 | /// joint's motion, i.e. Si * q_dot_i 126 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) Si_nui_s; 127 | 128 | /// \brief link spatial velocity expressed at the origin of the world. TODO: not used 129 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ovis; 130 | 131 | /// \brief link spatial velocity expressed in link's local frame from the previous solver 132 | /// iteration 133 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) vis_prev; 134 | 135 | /// \brief "state-feedback" matrix term in the recursion hyputhesis for f_i, analogous to 136 | /// articulated body inertia in [ABA] 137 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) His_aba; 138 | 139 | /// \brief initialized to 'Hi_inertias' but acts container for tmp quantities that gets 140 | /// overwritten during LOIK bwdpass 141 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) His; 142 | 143 | /// \brief dual variable projection terms 144 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) Pis; 145 | 146 | /// \brief bias vector terms in the recursion hyputhesis for f_i 147 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) pis; 148 | 149 | /// \brief intermediate recurssion term, stores each "articulated body inertia" transformed into 150 | /// the motion subspace of the parent joint 151 | PINOCCHIO_ALIGNED_STD_VECTOR(DMat) Dis; 152 | 153 | /// \brief inverse of the transformed articulated body inertias 154 | PINOCCHIO_ALIGNED_STD_VECTOR(DMat) Di_invs; 155 | 156 | /// \brief quadratic penalty term associated with nu_i introduced by inequality constraints on 157 | /// nu_i through ADMM 158 | PINOCCHIO_ALIGNED_STD_VECTOR(DMat) Ris; 159 | 160 | /// \brief affine terms associated with nu_i introduced by inequality constraints on nu_i 161 | /// through ADMM 162 | PINOCCHIO_ALIGNED_STD_VECTOR(DVec) ris; 163 | 164 | /// \brief dual variable for the kinematics/dynamics constraint, i.e. constraint "forces" 165 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) fis; 166 | 167 | // ADMM formulation related quantities 168 | 169 | /// \brief dual variables associated with motion constraints 170 | PINOCCHIO_ALIGNED_STD_VECTOR(DVec) yis; 171 | 172 | /// \brief previous dual variables associated with motion constraints 173 | PINOCCHIO_ALIGNED_STD_VECTOR(DVec) yis_prev; 174 | 175 | /// \brief dual vairables associated with inequality slack induced equality constraints 176 | DVec w; 177 | 178 | /// \brief previous dual vairables associated with inequality slack induced equality constraints 179 | DVec w_prev; 180 | 181 | /// \brief slack variables for inequality constraints 182 | DVec z; 183 | 184 | /// \brief previous slack variables for inequality constraints 185 | DVec z_prev; 186 | 187 | /// 188 | /// \brief untility member, all joint index collected into a std::vector, including the universe 189 | /// joint 190 | /// should be moved to Model 191 | /// 192 | IndexVector joint_full_range; 193 | 194 | /// 195 | /// \brief untility member, all joint index collected into a std::vector, excluding the universe 196 | /// joint 197 | /// should be moved to Model 198 | /// 199 | IndexVector joint_range; 200 | 201 | /// 202 | /// \brief dimension of the task equality constraints 203 | /// 204 | int eq_c_dim; 205 | 206 | /// 207 | /// \brief default constructor 208 | /// 209 | IkIdDataTpl() {}; 210 | 211 | /// 212 | /// \brief construct from pinocchio::ModelTpl 213 | /// 214 | /// \param[in] model The model structure of the rigid body system. 215 | /// 216 | explicit IkIdDataTpl(const Model & model); 217 | 218 | /// 219 | /// \brief construct from pinocchio::ModelTpl and a task equality constraint dimension 220 | /// 221 | /// \param[in] model The model structure of the rigid body system. 222 | /// \param[in] eq_c_dim Task Equality constraint dimension. 223 | /// 224 | explicit IkIdDataTpl(const Model & model, const int eq_c_dim); 225 | 226 | /// 227 | /// \brief reset all quantities 228 | /// 229 | void Reset(const bool warm_start); 230 | 231 | /// 232 | /// \brief reset all recursion related quantities 233 | /// 234 | void ResetRecursion(); 235 | 236 | /// 237 | /// \brief update _prev primal dual variables with that of current values 238 | /// 239 | void UpdatePrev(); 240 | 241 | }; // struct IkIdDataTpl 242 | 243 | /// 244 | /// \brief utility function for checking IkIdData against the Model it instantiated from 245 | /// 246 | template class JointCollectionTpl> 247 | inline bool checkIkIdData( 248 | const pinocchio::ModelTpl & model, 249 | const IkIdDataTpl & ik_id_data) 250 | { 251 | typedef pinocchio::ModelTpl Model; 252 | typedef IkIdDataTpl IkIdData; 253 | typedef typename Model::JointModel JointModel; 254 | 255 | std::cout << (static_cast(ik_id_data.joints.size()) == model.njoints) << std::endl; 256 | 257 | #define CHECK_DATA(a) \ 258 | if (!(a)) \ 259 | return false; 260 | 261 | // check sizes of each data members 262 | CHECK_DATA(static_cast(ik_id_data.joints.size()) == model.njoints); 263 | CHECK_DATA(static_cast(ik_id_data.oMi.size()) == model.njoints); 264 | CHECK_DATA(static_cast(ik_id_data.liMi.size()) == model.njoints); 265 | CHECK_DATA(static_cast(ik_id_data.nu.size()) == model.nv); 266 | CHECK_DATA(static_cast(ik_id_data.nu_prev.size()) == model.nv); 267 | CHECK_DATA(static_cast(ik_id_data.vis.size()) == model.njoints); 268 | CHECK_DATA(static_cast(ik_id_data.Si_nui_s.size()) == model.njoints); 269 | CHECK_DATA(static_cast(ik_id_data.ovis.size()) == model.njoints); 270 | CHECK_DATA(static_cast(ik_id_data.vis_prev.size()) == model.njoints); 271 | CHECK_DATA(static_cast(ik_id_data.His_aba.size()) == model.njoints); 272 | CHECK_DATA(static_cast(ik_id_data.His.size()) == model.njoints); 273 | CHECK_DATA(static_cast(ik_id_data.Pis.size()) == model.njoints); 274 | CHECK_DATA(static_cast(ik_id_data.pis.size()) == model.njoints); 275 | CHECK_DATA(static_cast(ik_id_data.Dis.size()) == model.njoints); 276 | CHECK_DATA(static_cast(ik_id_data.Di_invs.size()) == model.njoints); 277 | CHECK_DATA(static_cast(ik_id_data.Ris.size()) == model.njoints); 278 | CHECK_DATA(static_cast(ik_id_data.ris.size()) == model.njoints); 279 | CHECK_DATA(static_cast(ik_id_data.fis.size()) == model.njoints); 280 | CHECK_DATA(static_cast(ik_id_data.yis.size()) == model.njoints); 281 | CHECK_DATA(static_cast(ik_id_data.yis_prev.size()) == model.njoints); 282 | CHECK_DATA(static_cast(ik_id_data.w.size()) == model.nv); 283 | CHECK_DATA(static_cast(ik_id_data.w_prev.size()) == model.nv); 284 | CHECK_DATA(static_cast(ik_id_data.z.size()) == model.nv); 285 | CHECK_DATA(static_cast(ik_id_data.z_prev.size()) == model.nv); 286 | CHECK_DATA(static_cast(ik_id_data.joint_full_range.size()) == model.njoints); 287 | CHECK_DATA(static_cast(ik_id_data.joint_range.size()) == model.njoints - 1); 288 | 289 | for (pinocchio::JointIndex joint_id = 1; 290 | joint_id < static_cast(model.njoints); ++joint_id) 291 | { 292 | const JointModel & jmodel = model.joints[joint_id]; 293 | 294 | CHECK_DATA(model.nqs[joint_id] == jmodel.nq()); 295 | CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q()); 296 | CHECK_DATA(model.nvs[joint_id] == jmodel.nv()); 297 | CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v()); 298 | 299 | // check solver and recursion quantities dimensions 300 | CHECK_DATA(ik_id_data.yis[joint_id].size() == ik_id_data.eq_c_dim); 301 | CHECK_DATA(ik_id_data.Ris[joint_id].rows() == jmodel.nv()); 302 | CHECK_DATA(ik_id_data.Ris[joint_id].cols() == jmodel.nv()); 303 | CHECK_DATA(ik_id_data.ris[joint_id].size() == jmodel.nv()); 304 | CHECK_DATA(ik_id_data.Dis[joint_id].rows() == jmodel.nv()); 305 | CHECK_DATA(ik_id_data.Dis[joint_id].cols() == jmodel.nv()); 306 | CHECK_DATA(ik_id_data.Di_invs[joint_id].rows() == jmodel.nv()); 307 | CHECK_DATA(ik_id_data.Di_invs[joint_id].cols() == jmodel.nv()); 308 | } 309 | 310 | // check ik_id_data utility quantities 311 | typename IkIdData::IndexVector joint_full_range_test; 312 | joint_full_range_test.reserve(static_cast(model.njoints)); 313 | 314 | for (typename IkIdData::Index idx = 0; static_cast(idx) < model.njoints; idx++) 315 | { 316 | joint_full_range_test.push_back(idx); 317 | } 318 | 319 | typename IkIdData::IndexVector joint_range_test = joint_full_range_test; 320 | joint_range_test.erase(joint_range_test.begin()); 321 | 322 | CHECK_DATA(ik_id_data.joint_full_range == joint_full_range_test); 323 | CHECK_DATA(ik_id_data.joint_range == joint_range_test); 324 | 325 | #undef CHECK_DATA 326 | return true; 327 | } // checkIkIdData 328 | 329 | /// 330 | /// \brief utility function for checking IkIdData against pinocchio::Data 331 | /// 332 | template class JointCollectionTpl> 333 | inline bool checkIkIdData( 334 | const pinocchio::ModelTpl & model, 335 | const IkIdDataTpl & ik_id_data, 336 | const pinocchio::DataTpl & data) 337 | { 338 | 339 | bool size_dim_check = checkIkIdData(model, ik_id_data); 340 | 341 | if (size_dim_check) 342 | { 343 | #define CHECK_DATA(a) \ 344 | if (!(a)) \ 345 | return false; 346 | 347 | // check joint data created btw "ik_id_data" and "data" are the same 348 | for (std::size_t j = 1; j < static_cast(ik_id_data.joints.size()); ++j) 349 | { 350 | CHECK_DATA(ik_id_data.joints.at(j) == data.joints.at(j)); 351 | } 352 | 353 | #undef CHECK_DATA 354 | return true; 355 | } 356 | else 357 | { 358 | return false; 359 | } 360 | 361 | } // checkIkIdData 362 | 363 | } // namespace loik 364 | 365 | #include "loik/loik-loid-data.hxx" 366 | 367 | #if LOIK_ENABLE_TEMPLATE_INSTANTIATION 368 | #include "loik/loik-loid-data.txx" 369 | #endif // LOIK_ENABLE_TEMPLATE_INSTANTIATION 370 | -------------------------------------------------------------------------------- /include/loik/loik-loid-data-optimized.hxx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid-data-optimized.hpp" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace loik 17 | { 18 | 19 | /////////////////////////////////////////////////////////////////////////////////////////////// 20 | /////////////////////////////////////////////////////////////////////////////////////////////// 21 | /////////////////////////////////////////////////////////////////////////////////////////////// 22 | /// 23 | /// \brief IkIdDataTpl constructor with default constraint dimension of 6 24 | /// 25 | template class JointCollectionTpl> 26 | IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>::IkIdDataTypeOptimizedTpl( 27 | const Model & model) 28 | : IkIdDataTypeOptimizedTpl(model, 1) // delegating constructor call 29 | { 30 | // 31 | } 32 | 33 | /////////////////////////////////////////////////////////////////////////////////////////////// 34 | /////////////////////////////////////////////////////////////////////////////////////////////// 35 | /////////////////////////////////////////////////////////////////////////////////////////////// 36 | /// 37 | /// \brief IkIdDataTpl constructor 38 | /// 39 | template class JointCollectionTpl> 40 | IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>::IkIdDataTypeOptimizedTpl( 41 | const Model & model, const int num_eq_c) 42 | : joints(0) 43 | , oMi(static_cast(model.njoints), SE3::Identity()) 44 | , liMi(static_cast(model.njoints), SE3::Identity()) 45 | , nu(DVec::Zero(model.nv)) 46 | , nu_prev(DVec::Zero(model.nv)) 47 | , vis(static_cast(model.njoints), Motion::Zero()) 48 | , ovis(static_cast(model.njoints), Motion::Zero()) 49 | , vis_prev(static_cast(model.njoints), Motion::Zero()) 50 | , His(static_cast(model.njoints), Mat6x6::Identity()) 51 | , His_aba(static_cast(model.njoints), Mat6x6::Identity()) 52 | , pis(static_cast(model.njoints), Force::Zero()) 53 | , pis_aba(static_cast(model.njoints), Force::Zero()) 54 | , R(DVec::Zero(model.nv)) 55 | , r(DVec::Zero(model.nv)) 56 | , fis(static_cast(model.njoints), Force::Zero()) 57 | , delta_fis(static_cast(model.njoints), Force::Zero()) 58 | , yis(static_cast(num_eq_c), Vec6::Zero()) 59 | , delta_yis(static_cast(num_eq_c), Vec6::Zero()) 60 | , w(DVec::Zero(model.nv)) 61 | , delta_w(DVec::Zero(model.nv)) 62 | , z(DVec::Zero(model.nv)) 63 | , z_prev(DVec::Zero(model.nv)) 64 | , num_eq_c(num_eq_c) 65 | , Aty(static_cast(num_eq_c), Vec6::Zero()) 66 | , Aty_prev(static_cast(num_eq_c), Vec6::Zero()) 67 | , fis_diff_plus_Aty(static_cast(model.njoints), Force::Zero()) 68 | , delta_fis_diff_plus_Aty(static_cast(model.njoints), Force::Zero()) 69 | , Href_v(static_cast(model.njoints), Vec6::Zero()) 70 | , Av_minus_b(static_cast(num_eq_c), Vec6::Zero()) 71 | , Stf_plus_w(DVec::Zero(model.nv)) 72 | , delta_Stf_plus_w(DVec::Zero(model.nv)) 73 | , bT_delta_y_plus(0.0) 74 | , bT_delta_y_minus(0.0) 75 | , Av_inf_norm(0.0) 76 | , nu_inf_norm(0.0) 77 | , Href_v_inf_norm(0.0) 78 | , fis_diff_plus_Aty_inf_norm(0.0) 79 | , Stf_plus_w_inf_norm(0.0) 80 | , delta_fis_diff_plus_Aty_inf_norm(0.0) 81 | , delta_Stf_plus_w_inf_norm(0.0) 82 | , delta_vis_inf_norm(0.0) 83 | , delta_nu_inf_norm(0.0) 84 | , delta_z_inf_norm(0.0) 85 | , delta_fis_inf_norm(0.0) 86 | , delta_yis_inf_norm(0.0) 87 | , delta_w_inf_norm(0.0) 88 | { 89 | /* Create data structure associated to the joints */ 90 | for (JointIndex i = 0; i < static_cast(model.njoints); ++i) 91 | { 92 | // create joint datas 93 | joints.push_back( 94 | pinocchio::CreateJointData<_Scalar, _Options, JointCollectionTpl>::run(model.joints[i])); 95 | } 96 | 97 | // initialize utility members 98 | joint_full_range.reserve(static_cast(model.njoints)); 99 | joint_full_range.resize(static_cast(model.njoints)); 100 | joint_range.reserve(static_cast(model.njoints) - 1); 101 | joint_range.resize(static_cast(model.njoints) - 1); 102 | std::iota(joint_full_range.begin(), joint_full_range.end(), 0); // [0, nb_ - 1] 103 | std::iota(joint_range.begin(), joint_range.end(), 1); // [1, nb_ - 1] 104 | } 105 | 106 | /////////////////////////////////////////////////////////////////////////////////////////////// 107 | /////////////////////////////////////////////////////////////////////////////////////////////// 108 | /////////////////////////////////////////////////////////////////////////////////////////////// 109 | /// 110 | /// \brief reset all quantities, called by solver during SolveInit(), only called once per problem 111 | /// 112 | template class JointCollectionTpl> 113 | void IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>::Reset(const bool warm_start) 114 | { 115 | 116 | if (!warm_start) 117 | { 118 | w.setZero(); 119 | z.setZero(); 120 | nu.setZero(); 121 | for (auto & idx : joint_full_range) 122 | { 123 | vis[idx].setZero(); 124 | fis[idx].setZero(); 125 | fis_diff_plus_Aty[idx].setZero(); 126 | } 127 | } 128 | } // Reset 129 | 130 | /////////////////////////////////////////////////////////////////////////////////////////////// 131 | /////////////////////////////////////////////////////////////////////////////////////////////// 132 | /////////////////////////////////////////////////////////////////////////////////////////////// 133 | /// 134 | /// \brief reset only recursion related quantities, called once at the begining of Solve(), not 135 | /// resetting oMi and liMi, i.e. assuming robot state hasn't changed 136 | /// 137 | template class JointCollectionTpl> 138 | void IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>::ResetRecursion() 139 | { 140 | w.setZero(); 141 | z.setZero(); 142 | 143 | for (auto & idx : joint_full_range) 144 | { 145 | vis[idx].setZero(); 146 | fis[idx].setZero(); 147 | fis_diff_plus_Aty[idx].setZero(); 148 | } 149 | 150 | for (int idx = 0; idx < num_eq_c; idx++) 151 | { 152 | yis[idx].setZero(); 153 | Aty[idx].setZero(); 154 | } 155 | 156 | } // ResetRecursion 157 | 158 | /////////////////////////////////////////////////////////////////////////////////////////////// 159 | /////////////////////////////////////////////////////////////////////////////////////////////// 160 | /////////////////////////////////////////////////////////////////////////////////////////////// 161 | /// 162 | /// \brief reset only inf norm related quantities, called at the begining of each solver inner 163 | /// iterations 164 | /// 165 | template class JointCollectionTpl> 166 | void IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>::ResetInfNorms() 167 | { 168 | bT_delta_y_plus = 0.0; 169 | bT_delta_y_minus = 0.0; 170 | Av_inf_norm = 0.0; 171 | nu_inf_norm = 0.0; 172 | Href_v_inf_norm = 0.0; 173 | fis_diff_plus_Aty_inf_norm = 0.0; 174 | Stf_plus_w_inf_norm = 0.0; 175 | delta_fis_diff_plus_Aty_inf_norm = 0.0; 176 | delta_Stf_plus_w_inf_norm = 0.0; 177 | delta_vis_inf_norm = 0.0; 178 | delta_nu_inf_norm = 0.0; 179 | delta_z_inf_norm = 0.0; 180 | delta_fis_inf_norm = 0.0; 181 | delta_yis_inf_norm = 0.0; 182 | delta_w_inf_norm = 0.0; 183 | } // ResetInfNorms 184 | 185 | /////////////////////////////////////////////////////////////////////////////////////////////// 186 | /////////////////////////////////////////////////////////////////////////////////////////////// 187 | /////////////////////////////////////////////////////////////////////////////////////////////// 188 | /// 189 | /// \brief update _prev primal dual variables with that of current values 190 | /// 191 | template class JointCollectionTpl> 192 | void IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>::UpdatePrev() 193 | { 194 | vis_prev = vis; 195 | nu_prev = nu; 196 | z_prev = z; 197 | } // UpdatePrev 198 | 199 | /////////////////////////////////////////////////////////////////////////////////////////////// 200 | /////////////////////////////////////////////////////////////////////////////////////////////// 201 | /////////////////////////////////////////////////////////////////////////////////////////////// 202 | /// 203 | /// \brief operator "==" overload, as a Non-member function for symmetric behavior, i.e. a == b 204 | /// and b == a shouldm give the same result. 205 | /// 206 | template class JointCollectionTpl> 207 | bool operator==( 208 | const IkIdDataTypeOptimizedTpl & ik_id_data_1, 209 | const IkIdDataTypeOptimizedTpl & ik_id_data_2) 210 | { 211 | bool value = 212 | ik_id_data_1.joints == ik_id_data_2.joints && ik_id_data_1.oMi == ik_id_data_2.oMi 213 | && ik_id_data_1.liMi == ik_id_data_2.liMi && ik_id_data_1.nu == ik_id_data_2.nu 214 | && ik_id_data_1.nu_prev == ik_id_data_2.nu_prev && ik_id_data_1.vis == ik_id_data_2.vis 215 | && ik_id_data_1.ovis == ik_id_data_2.ovis && ik_id_data_1.vis_prev == ik_id_data_2.vis_prev 216 | && ik_id_data_1.His_aba == ik_id_data_2.His_aba && ik_id_data_1.His == ik_id_data_2.His 217 | && ik_id_data_1.pis == ik_id_data_2.pis && ik_id_data_1.pis_aba == ik_id_data_2.pis_aba 218 | && ik_id_data_1.R == ik_id_data_2.R && ik_id_data_1.r == ik_id_data_2.r 219 | && ik_id_data_1.fis == ik_id_data_2.fis && ik_id_data_1.delta_fis == ik_id_data_2.delta_fis 220 | && ik_id_data_1.yis == ik_id_data_2.yis && ik_id_data_1.delta_yis == ik_id_data_2.delta_yis 221 | && ik_id_data_1.w == ik_id_data_2.w && ik_id_data_1.delta_w == ik_id_data_2.delta_w 222 | && ik_id_data_1.z == ik_id_data_2.z && ik_id_data_1.z_prev == ik_id_data_2.z_prev 223 | && ik_id_data_1.joint_full_range == ik_id_data_2.joint_full_range 224 | && ik_id_data_1.joint_range == ik_id_data_2.joint_range 225 | && ik_id_data_1.Aty == ik_id_data_2.Aty && ik_id_data_1.Aty_prev == ik_id_data_2.Aty_prev 226 | && ik_id_data_1.fis_diff_plus_Aty == ik_id_data_2.fis_diff_plus_Aty 227 | && ik_id_data_1.delta_fis_diff_plus_Aty == ik_id_data_2.delta_fis_diff_plus_Aty 228 | && ik_id_data_1.Href_v == ik_id_data_2.Href_v 229 | && ik_id_data_1.Av_minus_b == ik_id_data_2.Av_minus_b 230 | && ik_id_data_1.Stf_plus_w == ik_id_data_2.Stf_plus_w 231 | && ik_id_data_1.delta_Stf_plus_w == ik_id_data_2.delta_Stf_plus_w 232 | && ik_id_data_1.bT_delta_y_plus == ik_id_data_2.bT_delta_y_plus 233 | && ik_id_data_1.bT_delta_y_minus == ik_id_data_2.minus 234 | && ik_id_data_1.Av_inf_norm == ik_id_data_2.Av_inf_norm 235 | && ik_id_data_1.nu_inf_norm == ik_id_data_2.nu_inf_norm 236 | && ik_id_data_1.Href_v_inf_norm == ik_id_data_2.Href_v_inf_norm 237 | && ik_id_data_1.fis_diff_plus_Aty_inf_norm == ik_id_data_2.fis_diff_plus_Aty_inf_norm 238 | && ik_id_data_1.Stf_plus_w_inf_norm == ik_id_data_2.Stf_plus_w_inf_norm 239 | && ik_id_data_1.delta_fis_diff_plus_Aty_inf_norm 240 | == ik_id_data_2.delta_fis_diff_plus_Aty_inf_norm 241 | && ik_id_data_1.delta_Stf_plus_w_inf_norm == ik_id_data_2.delta_Stf_plus_w_inf_norm 242 | && ik_id_data_1.delta_vis_inf_norm == ik_id_data_2.delta_vis_inf_norm 243 | && ik_id_data_1.delta_nu_inf_norm == ik_id_data_2.delta_nu_inf_norm 244 | && ik_id_data_1.delta_z_inf_norm == ik_id_data_2.delta_z_inf_norm 245 | && ik_id_data_1.delta_fis_inf_norm == ik_id_data_2.delta_fis_inf_norm 246 | && ik_id_data_1.delta_yis_inf_norm == ik_id_data_2.delta_yis_inf_norm 247 | && ik_id_data_1.delta_w_inf_norm == ik_id_data_2.delta_w_inf_norm; 248 | 249 | return value; 250 | } 251 | 252 | /////////////////////////////////////////////////////////////////////////////////////////////// 253 | /////////////////////////////////////////////////////////////////////////////////////////////// 254 | /////////////////////////////////////////////////////////////////////////////////////////////// 255 | /// 256 | /// \brief operator "!=" overload, as a Non-member function for symmetric behavior, i.e. a != b 257 | /// and b != a shouldm give the same result. 258 | /// 259 | template class JointCollectionTpl> 260 | bool operator!=( 261 | const IkIdDataTypeOptimizedTpl & ik_id_data_1, 262 | const IkIdDataTypeOptimizedTpl & ik_id_data_2) 263 | { 264 | return !(ik_id_data_1 == ik_id_data_2); 265 | } 266 | 267 | /////////////////////////////////////////////////////////////////////////////////////////////// 268 | /////////////////////////////////////////////////////////////////////////////////////////////// 269 | /////////////////////////////////////////////////////////////////////////////////////////////// 270 | /// 271 | /// \brief utility function for defining symmetric operator "==" overload between IkIdData and 272 | /// Data 273 | /// 274 | template class JointCollectionTpl> 275 | bool CompareDataIkIdData( 276 | const IkIdDataTypeOptimizedTpl & ik_id_data, 277 | const pinocchio::DataTpl & data) 278 | { 279 | bool value = ik_id_data.oMi == data.oMi && ik_id_data.liMi == data.liMi; 280 | 281 | // check joint data created btw "ik_id_data" and "data" are the same 282 | for (std::size_t j = 1; j < static_cast(ik_id_data.joints.size()); ++j) 283 | { 284 | value &= ik_id_data.joints.at(j) == data.joints.at(j); 285 | } 286 | 287 | return value; 288 | } 289 | 290 | /////////////////////////////////////////////////////////////////////////////////////////////// 291 | /////////////////////////////////////////////////////////////////////////////////////////////// 292 | /////////////////////////////////////////////////////////////////////////////////////////////// 293 | /// 294 | /// \brief symmetric operator "==" overloads between IkIdData and Data 295 | /// 296 | template class JointCollectionTpl> 297 | bool operator==( 298 | const IkIdDataTypeOptimizedTpl & ik_id_data, 299 | const pinocchio::DataTpl & data) 300 | { 301 | return CompareDataIkIdData(ik_id_data, data); 302 | } 303 | 304 | template class JointCollectionTpl> 305 | bool operator==( 306 | const pinocchio::DataTpl & data, 307 | const IkIdDataTypeOptimizedTpl & ik_id_data) 308 | { 309 | return CompareDataIkIdData(ik_id_data, data); 310 | } 311 | 312 | /////////////////////////////////////////////////////////////////////////////////////////////// 313 | /////////////////////////////////////////////////////////////////////////////////////////////// 314 | /////////////////////////////////////////////////////////////////////////////////////////////// 315 | /// 316 | /// \brief symmetric operator "!=" overloads between IkIdData and Data 317 | /// 318 | template class JointCollectionTpl> 319 | bool operator!=( 320 | const IkIdDataTypeOptimizedTpl & ik_id_data, 321 | const pinocchio::DataTpl & data) 322 | { 323 | return !CompareDataIkIdData(ik_id_data, data); 324 | } 325 | 326 | template class JointCollectionTpl> 327 | bool operator!=( 328 | const pinocchio::DataTpl & data, 329 | const IkIdDataTypeOptimizedTpl & ik_id_data) 330 | { 331 | return !CompareDataIkIdData(ik_id_data, data); 332 | } 333 | 334 | } // namespace loik 335 | -------------------------------------------------------------------------------- /include/loik/ik-id-description-optimized.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/macros.hpp" 8 | #include "loik/loik-loid-data-optimized.hpp" 9 | 10 | #include 11 | #include 12 | 13 | namespace loik 14 | { 15 | 16 | template 17 | struct IkProblemFormulationOptimized 18 | { 19 | using IkIdDataOptimized = IkIdDataTypeOptimizedTpl<_Scalar>; 20 | using Motion = typename IkIdDataOptimized::Motion; 21 | using Force = typename IkIdDataOptimized::Force; 22 | using DMat = typename IkIdDataOptimized::DMat; 23 | using DVec = typename IkIdDataOptimized::DVec; 24 | using Vec3 = typename IkIdDataOptimized::Vec3; 25 | using Vec6 = typename IkIdDataOptimized::Vec6; 26 | using Mat6x6 = typename IkIdDataOptimized::Mat6x6; 27 | using Index = typename IkIdDataOptimized::Index; 28 | using IndexVec = typename IkIdDataOptimized::IndexVector; 29 | 30 | explicit IkProblemFormulationOptimized( 31 | const int nj, const int nb, const int nc_eq, const int eq_c_dim, const int ineq_c_dim) 32 | : eq_c_dim_(eq_c_dim) 33 | , nj_(nj) 34 | , nb_(nb) 35 | , nc_eq_(nc_eq) 36 | , ineq_c_dim_(ineq_c_dim) 37 | { 38 | if (nj_ != nb_ + 1) 39 | { 40 | throw(std::runtime_error( 41 | "[IkProblemFormulation::IkProblemFormulation]: nb does not equal " 42 | "to nj - 1, robot model not supported !!!")); 43 | } 44 | 45 | if (eq_c_dim != 6) 46 | { 47 | throw(std::runtime_error( 48 | "[IkProblemFormulation::IkProblemFormulation]: equality constraint " 49 | "dimension is not 6, problem formulation not supported !!!")); 50 | } 51 | 52 | H_refs_.reserve(static_cast(nj)); 53 | v_refs_.reserve(static_cast(nj)); 54 | 55 | active_task_constraint_ids_.reserve(static_cast(nj)); 56 | Ais_.reserve(static_cast(nj)); 57 | bis_.reserve(static_cast(nj)); 58 | 59 | lb_ = Eigen::VectorXd::Zero(ineq_c_dim_); 60 | ub_ = Eigen::VectorXd::Zero(ineq_c_dim_); 61 | 62 | Reset(); 63 | }; 64 | 65 | void Reset() 66 | { 67 | 68 | // reset reference 69 | ResetReferences(); 70 | 71 | // reset equality constraints 72 | ResetEqConstraints(); 73 | 74 | // reset inequality constraints 75 | ResetIneqConstraints(); 76 | }; 77 | 78 | /// 79 | /// \brief set tracking reference by duplicating weights and targets for all body links 80 | /// 81 | void UpdateReference(const Mat6x6 & H_ref_target, const Motion & v_ref_target) 82 | { 83 | // check H_refs_ is not empty 84 | if (H_refs_.size() <= 0) 85 | { 86 | throw(std::runtime_error( 87 | "[IkProblemFormulation::UpdateReference]: 'H_refs_' and 'v_refs_' are empty")); 88 | } 89 | 90 | for (Index idx = 0; idx < H_refs_.size(); idx++) 91 | { 92 | H_refs_[idx] = H_ref_target; 93 | v_refs_[idx] = v_ref_target; 94 | 95 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 96 | Hv[idx] = H_refs_[idx] * v_refs_[idx].toVector(); 97 | LOIK_EIGEN_MALLOC_ALLOWED(); 98 | } 99 | 100 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 101 | Hv_inf_norm_ = Hv[0].template lpNorm(); 102 | LOIK_EIGEN_MALLOC_ALLOWED(); 103 | }; 104 | 105 | /// 106 | /// \brief set tracking references uisng vectors of references 107 | /// 108 | void UpdateReferences( 109 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & H_refs, 110 | const PINOCCHIO_ALIGNED_STD_VECTOR(Motion) & v_refs) 111 | { 112 | if ( 113 | H_refs.size() != static_cast(nj_) 114 | || v_refs.size() != static_cast(nj_)) 115 | { 116 | throw(std::runtime_error( 117 | "[IkProblemFormulation::UpdateReferences]: input arguments " 118 | "'H_refs', 'v_refs' have wrong size!!")); 119 | } 120 | H_refs_ = H_refs; 121 | v_refs_ = v_refs; 122 | 123 | for (Index idx = 0; idx < H_refs_.size(); idx++) 124 | { 125 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 126 | Hv[idx] = H_refs_[idx] * v_refs_[idx].toVector(); 127 | 128 | if (Hv[idx].template lpNorm() > Hv_inf_norm_) 129 | { 130 | Hv_inf_norm_ = Hv[idx].template lpNorm(); 131 | } 132 | 133 | LOIK_EIGEN_MALLOC_ALLOWED(); 134 | } 135 | }; 136 | 137 | /// 138 | /// \brief batch update equality constraints, number of equality constraints and constraint 139 | /// dimensions must not change 140 | /// 141 | void UpdateEqConstraints( 142 | const std::vector & active_task_constraint_ids, 143 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & Ais, 144 | const PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) & bis) 145 | { 146 | // check constraint specification is consistant 147 | if (!((active_task_constraint_ids.size() == Ais.size()) && ((Ais.size() == bis.size())))) 148 | { 149 | throw(std::runtime_error( 150 | "[IkProblemFormulation::UpdateEqConstraints]: " 151 | "task_constraint_ids, Ais, and bis have different size !!!")); 152 | } 153 | 154 | // check constraint dimension is consistent between input arguments! 155 | if (Ais[0].rows() != bis[0].rows()) 156 | { 157 | throw(std::runtime_error( 158 | "[IkProblemFormulation::UpdateEqConstraints] Ai and bi dimension mismatch!!")); 159 | } 160 | 161 | // check if number of equality constraints has changed 162 | if (active_task_constraint_ids.size() != static_cast(nc_eq_)) 163 | { 164 | throw(std::runtime_error( 165 | "[IkProblemFormulation::UpdateEqConstraints]: number of equality " 166 | "constraints doesn't match initialization!!!")); 167 | } 168 | 169 | // check if equality constraint dim has changed 170 | if (Ais[0].rows() != eq_c_dim_) 171 | { 172 | // TODO: need to check constraint dimension for each Ai and bi, not just the first ones 173 | throw(std::runtime_error( 174 | "[IkProblemFormulation::UpdateEqConstraints]: equality constraint " 175 | "dimension has changed!!! Updating constraint dimension")); 176 | } 177 | 178 | active_task_constraint_ids_ = active_task_constraint_ids; 179 | Ais_ = Ais; 180 | bis_ = bis; 181 | 182 | // set bi_inf_norm to zero just to be sure 183 | bis_inf_norm_ = 0.0; 184 | 185 | for (Index idx = 0; idx < Ais_.size(); idx++) 186 | { 187 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 188 | AtA[idx] = Ais_[idx].transpose() * Ais_[idx]; 189 | Atb[idx] = Ais_[idx].transpose() * bis_[idx]; 190 | 191 | if (bis_[idx].template lpNorm() > bis_inf_norm_) 192 | { 193 | bis_inf_norm_ = bis_[idx].template lpNorm(); 194 | } 195 | 196 | LOIK_EIGEN_MALLOC_ALLOWED(); 197 | } 198 | }; 199 | 200 | /// 201 | /// \brief set equality constraints by individual link id and updating 'Ai' and 'bi', the link 202 | /// id to update must already be 203 | /// present in 'active_task_constraint_ids_'. 204 | /// 205 | void UpdateEqConstraint(const Index c_id, const Mat6x6 & Ai, const Vec6 & bi) 206 | { 207 | // check if constraint already exist at link 'c_id' 208 | auto found_it = 209 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 210 | 211 | // if 'c_id' not present in 'active_task_constraint_ids_', invoke 'AddEqConstraint()' 212 | if (found_it == active_task_constraint_ids_.end()) 213 | { 214 | throw(std::runtime_error( 215 | "[IkProblemFormulation::UpdateEqConstraint]: constraint doesn't " 216 | "yet exist at link 'c_id' !!! ")); 217 | } 218 | 219 | // check if constraint dimension is consistent 220 | if (Ai.rows() != bi.size() || Ai.rows() != static_cast(eq_c_dim_)) 221 | { 222 | throw(std::runtime_error( 223 | "[IkProblemFormulation::UpdateEqConstraint]: constraint dimension inconsistent!!!")); 224 | } 225 | 226 | // count how many times 'c_id' appear in 'active_task_constraint_ids_' 227 | int c_id_count = 228 | std::count(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 229 | 230 | // if 'c_id' appear more than once, then something went wrong, throw. 231 | if (c_id_count > 1) 232 | { 233 | throw(std::runtime_error( 234 | "[IkProblemFormulation::UpdateEqConstraint]: multiple constraint " 235 | "specification for the same link id, not supported, terminating !!!")); 236 | } 237 | 238 | // get index of 'c_id' in 'active_task_constraint_ids_' 239 | Index c_id_in_vec = std::distance(active_task_constraint_ids_.begin(), found_it); 240 | 241 | // update Ai 242 | Ais_[c_id_in_vec] = Ai; 243 | // update bi 244 | bis_[c_id_in_vec] = bi; 245 | // udpate utility 246 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 247 | AtA[c_id_in_vec] = Ai.transpose() * Ai; 248 | Atb[c_id_in_vec] = Ai.transpose() * bi; 249 | 250 | if (bi.template lpNorm() > bis_inf_norm_) 251 | { 252 | bis_inf_norm_ = bi.template lpNorm(); 253 | } 254 | LOIK_EIGEN_MALLOC_ALLOWED(); 255 | }; 256 | 257 | /// 258 | /// \brief update equality constriant by link id, update 'bi' only, keep using old 'Ai', 259 | /// constraint must already be present for link id 260 | /// 261 | void UpdateEqConstraint(const Index c_id, const Vec6 & bi) 262 | { 263 | // check if constraint already exist at link 'c_id' 264 | auto found_it = 265 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 266 | 267 | // if 'c_id' not present in 'active_task_constraint_ids_', then throw 268 | if (found_it == active_task_constraint_ids_.end()) 269 | { 270 | throw(std::runtime_error( 271 | "[IkProblemFormulation::UpdateEqConstraint]: constraint doesn't " 272 | "yet exist at link 'c_id' !!! ")); 273 | } 274 | 275 | // get index of 'c_id' in 'active_task_constraint_ids_' 276 | Index c_id_in_vec = std::distance(active_task_constraint_ids_.begin(), found_it); 277 | UpdateEqConstraint(c_id, Ais_[c_id_in_vec], bi); 278 | } 279 | 280 | /// 281 | /// \brief (deactivated for now) add equality constraint by link id. If constraint is already 282 | /// present for link id, then invoke 'UpdateEqConstraint()' 283 | /// 284 | void AddEqConstraint(const Index c_id, const Mat6x6 & Ai, const Vec6 & bi) 285 | { 286 | // check if 'c_id' in 'active_task_constraint_ids_' 287 | auto found_it = 288 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 289 | 290 | // if found, update instead 291 | if (found_it != active_task_constraint_ids_.end()) 292 | { 293 | // constraint already defined at link id, updating instead 294 | UpdateEqConstraint(c_id, Ai, bi); 295 | } 296 | else 297 | { 298 | // not found 299 | 300 | // check constraint dimension 301 | if (Ai.rows() != bi.rows()) 302 | { 303 | throw(std::runtime_error( 304 | "[IkProblemFormulation::AddEqConstraint]: input arguments " 305 | "constraint dimension inconsistent !!!")); 306 | } 307 | 308 | if (Ai.rows() != static_cast(eq_c_dim_)) 309 | { 310 | throw(std::runtime_error( 311 | "[IkProblemFormulation::AddEqConstraint]: input constraint " 312 | "dimension differ from existing constriant dimension!!!")); 313 | } 314 | 315 | active_task_constraint_ids_.push_back(c_id); 316 | nc_eq_++; 317 | 318 | // add Ai and bi 319 | Ais_.push_back(Ai); 320 | bis_.push_back(bi); 321 | 322 | // add to utility 323 | // LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 324 | AtA.push_back(Ai.transpose() * Ai); 325 | Atb.push_back(Ai.transpose() * bi); 326 | // LOIK_EIGEN_MALLOC_ALLOWED(); 327 | 328 | // check bis_inf_norm 329 | if (bi.template lpNorm() > bis_inf_norm_) 330 | { 331 | bis_inf_norm_ = bi.template lpNorm(); 332 | } 333 | } 334 | }; 335 | 336 | /// 337 | /// \brief (deactivated for now) remove equality constraint by link id, if constraint not 338 | /// already present for link id, then do nothing 339 | /// 340 | void RemoveEqConstraint(const Index c_id) 341 | { 342 | // check if constraint already exist at link 'c_id' 343 | auto found_it = 344 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 345 | 346 | if (found_it == active_task_constraint_ids_.end()) 347 | { 348 | // no constraint defined at input link id, do nothing 349 | std::cerr << "WARNING [IkProblemFormulation::RemoveEqConstraint]: no constraint defined at " 350 | "link id, nothing to remove. " 351 | << std::endl; 352 | return; 353 | } 354 | 355 | // get index of 'c_id' in 'active_task_constraint_ids_' 356 | active_task_constraint_ids_.erase(found_it); 357 | Ais_.erase(found_it); 358 | bis_.erase(found_it); 359 | AtA.erase(found_it); 360 | Atb.erase(found_it); 361 | 362 | bis_inf_norm_ = 0.0; 363 | for (Index idx = 0; idx < Ais_.size(); idx++) 364 | { 365 | if (bis_[idx].template lpNorm() > bis_inf_norm_) 366 | { 367 | bis_inf_norm_ = bis_[idx].template lpNorm(); 368 | } 369 | } 370 | // Aty.erase(found_it); 371 | nc_eq_--; 372 | }; 373 | 374 | /// 375 | /// \brief set inequality constraints 376 | /// 377 | void UpdateIneqConstraints(const DVec & lb, const DVec & ub) 378 | { 379 | // check arguments have some dimension 380 | if (lb.size() != ub.size()) 381 | { 382 | throw(std::runtime_error( 383 | "[IkProblemFormulation::UpdateIneqConstraints]: lower bound and " 384 | "upper bound have different dimensions!!!")); 385 | } 386 | 387 | // check is inequality constraint dimension has changed 388 | if (lb.size() != ineq_c_dim_) 389 | { 390 | throw(std::runtime_error( 391 | "IkProblemFormulation::UpdateIneqConstraints]: inequality constraint " 392 | "dimension has changed, this is not supported currently!!!")); 393 | } 394 | 395 | lb_ = lb; 396 | ub_ = ub; 397 | }; 398 | 399 | int eq_c_dim_; // constraint dimension of the equality task constraint, this is shared across 400 | // all eq constraints 401 | int nj_; // number of joints 402 | int nb_; // number of bodies 403 | int nc_eq_; // number of equality constraints in the problem 404 | int ineq_c_dim_; // inequality constraint dimension, == pinocchio Model::nv 405 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) 406 | H_refs_; // weights for spatial velocity reference tracking in the cost, i.e. soft constraints 407 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v_refs_; // spatial velocity tracking references 408 | std::vector 409 | active_task_constraint_ids_; // vector of idx of links with active hard constraints 410 | 411 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) 412 | Ais_; // 'A' terms in the active equality constraint definitions 413 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) 414 | bis_; // 'b' terms in the active equality constraint definitions 415 | DVec lb_; // lower bound for primal decision variables 416 | DVec ub_; // upper bound for primal decision variables 417 | 418 | // utility members 419 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) AtA; // Ai.transpose * Ai ; updated by UpdateEqConstraint 420 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Atb; // Ai.transpose * bi ; updated by UpdateEqConstraint 421 | // PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Aty; // Ai.transpose * yi ; updated in the solver 422 | // after dual variables (yis) udpate 423 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Hv; // Hi_ref_ * vi_ref ; updated by UpdateReferences 424 | _Scalar bis_inf_norm_; // infinity norm of bis viewd as a vector 425 | _Scalar Hv_inf_norm_; // inf norm of Href * vref viewd as a vector 426 | 427 | private: 428 | /// 429 | /// \brief reset tracking references weights and targets 430 | /// 431 | void ResetReferences() 432 | { 433 | H_refs_.clear(); 434 | v_refs_.clear(); 435 | Hv.clear(); 436 | H_refs_.resize(static_cast(nj_)); 437 | v_refs_.resize(static_cast(nj_)); 438 | Hv.resize(static_cast(nj_)); 439 | for (Index idx = 0; idx < static_cast(nj_); idx++) 440 | { 441 | H_refs_[idx].setZero(); 442 | v_refs_[idx].setZero(); 443 | Hv[idx].setZero(); 444 | } 445 | Hv_inf_norm_ = 0.0; 446 | } 447 | 448 | /// 449 | /// \brief reset equality constraint quantities, 'Ais_' and 'bis_' 450 | /// 451 | void ResetEqConstraints() 452 | { 453 | active_task_constraint_ids_.clear(); 454 | Ais_.clear(); 455 | bis_.clear(); 456 | AtA.clear(); 457 | Atb.clear(); 458 | bis_inf_norm_ = 0.0; 459 | // Aty.clear(); 460 | 461 | active_task_constraint_ids_.resize(static_cast(nc_eq_)); 462 | Ais_.resize(static_cast(nc_eq_)); 463 | bis_.resize(static_cast(nc_eq_)); 464 | AtA.resize(static_cast(nc_eq_)); 465 | Atb.resize(static_cast(nc_eq_)); 466 | 467 | for (Index idx = 0; idx < static_cast(nc_eq_); idx++) 468 | { 469 | active_task_constraint_ids_[idx] = 0; 470 | Ais_[idx].setZero(); 471 | bis_[idx].setZero(); 472 | AtA[idx].setZero(); 473 | Atb[idx].setZero(); 474 | } 475 | } 476 | 477 | /// 478 | /// \brief reset inequality constriant quantities, 'lb_' and 'ub_' 479 | /// 480 | void ResetIneqConstraints() 481 | { 482 | lb_.setZero(); 483 | ub_.setZero(); 484 | } 485 | 486 | }; // struct IkProblemFormulationOptimized 487 | 488 | } // namespace loik 489 | -------------------------------------------------------------------------------- /include/loik/loik-loid-data-optimized.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "fwd.hpp" 8 | #include "loik/config.hpp" 9 | #include "loik/macros.hpp" 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | // namespace loik 36 | // { 37 | 38 | // template< 39 | // typename _Scalar, 40 | // int _Options = 0, 41 | // template class JointCollectionTpl = pinocchio::JointCollectionDefaultTpl> 42 | // struct IkIdDataTypeOptimizedTpl; 43 | 44 | // } 45 | 46 | namespace pinocchio 47 | { 48 | 49 | template class JointCollectionTpl> 50 | struct traits> 51 | { 52 | typedef _Scalar Scalar; 53 | }; 54 | 55 | } // namespace pinocchio 56 | 57 | namespace loik 58 | { 59 | 60 | // IkIdDataTpl definition 61 | template class JointCollectionTpl> 62 | struct IkIdDataTypeOptimizedTpl 63 | : pinocchio::serialization::Serializable< 64 | IkIdDataTypeOptimizedTpl<_Scalar, _Options, JointCollectionTpl>> 65 | , pinocchio::NumericalBase> 66 | { 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | 69 | typedef _Scalar Scalar; 70 | enum 71 | { 72 | Options = _Options 73 | }; 74 | 75 | typedef pinocchio::ModelTpl Model; 76 | 77 | typedef pinocchio::SE3Tpl SE3; 78 | typedef pinocchio::MotionTpl Motion; 79 | typedef pinocchio::ForceTpl Force; 80 | typedef pinocchio::InertiaTpl Inertia; 81 | typedef pinocchio::FrameTpl Frame; 82 | 83 | typedef pinocchio::Index Index; 84 | typedef pinocchio::JointIndex JointIndex; 85 | typedef pinocchio::FrameIndex FrameIndex; 86 | typedef std::vector IndexVector; 87 | 88 | typedef pinocchio::JointModelTpl JointModel; 89 | typedef pinocchio::JointDataTpl JointData; 90 | 91 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; 92 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; 93 | 94 | typedef Eigen::Matrix DMat; 95 | typedef Eigen::Matrix DVec; 96 | typedef Eigen::Matrix Vec3; 97 | typedef Eigen::Matrix Vec6; 98 | typedef Eigen::Matrix Mat6x6; 99 | 100 | /// \brief Dense vectorized version of a joint configuration vector, q. 101 | typedef DVec ConfigVectorType; 102 | 103 | /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, 104 | /// etc), q_dot. 105 | /// It also handles the notion of co-tangent vector (e.g. torque, etc). 106 | typedef DVec TangentVectorType; 107 | 108 | /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in 109 | /// model, encapsulated in JointDataAccessor. 110 | JointDataVector joints; 111 | 112 | /// \brief Vector of absolute joint placements (wrt the world). 113 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi; 114 | 115 | /// \brief Vector of relative joint placements (wrt the body parent). 116 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi; 117 | 118 | /// \brief generalized velocity 119 | DVec nu; 120 | 121 | /// \brief generalized velocity from the previous solver iteration 122 | DVec nu_prev; 123 | 124 | /// \brief link spatial velocity expressed in link's local frame 125 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) vis; 126 | 127 | /// \brief link spatial velocity expressed at the origin of the world. TODO: not used 128 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ovis; 129 | 130 | /// \brief link spatial velocity expressed in link's local frame from the previous solver 131 | /// iteration 132 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) vis_prev; 133 | 134 | /// \brief "state-feedback" matrix term in the recursion hyputhesis for f_i 135 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) His; 136 | 137 | /// \brief initialized to 'His' but acts as a container for tmp quantities that gets overwritten 138 | /// during LOIK bwdpass, after bwdpass, it becomes Pi * Hi, analogous to 'data.Yaba' 139 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) His_aba; 140 | 141 | /// \brief bias vector terms in the recursion hypothesis for f_i 142 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) pis; 143 | 144 | /// \brief bias vector terms in the recursion hypothesis for f_i 145 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) pis_aba; 146 | 147 | /// \brief quadratic penalty term associated with nu_i introduced by inequality constraints on 148 | /// nu_i through ADMM, equivalent to armature 149 | DVec R; 150 | 151 | /// \brief affine terms associated with nu_i introduced by inequality constraints on nu_i 152 | /// through ADMM, type joint space "force" 153 | /// analogous to 'data.u' 154 | DVec r; 155 | 156 | /// \brief dual variable for the kinematics/dynamics constraint, i.e. constraint "forces" 157 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) fis; 158 | 159 | /// \brief delta of fis, computed in BwdPass1 160 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) delta_fis; 161 | 162 | // ADMM formulation related quantities 163 | 164 | /// \brief dual variables associated with motion constraints 165 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) yis; 166 | 167 | /// \brief delta of yis, 168 | /// delta_yi = mu_eq * (A * vi - bi); 169 | /// computed at each iteration during DualUpdate 170 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) delta_yis; 171 | 172 | /// \brief dual vairables associated with inequality slack induced equality constraints 173 | DVec w; 174 | 175 | /// \brief delta of w, 176 | /// delta_w = mu_ineq * (nu - z); 177 | /// computed at each iteration during DualUpdate 178 | DVec delta_w; 179 | 180 | /// \brief slack variables for inequality constraints 181 | DVec z; 182 | 183 | /// \brief previous slack variables for inequality constraints 184 | DVec z_prev; 185 | 186 | /// 187 | /// \brief untility member, all joint index collected into a std::vector, including the universe 188 | /// joint 189 | /// should be moved to Model 190 | /// 191 | IndexVector joint_full_range; 192 | 193 | /// 194 | /// \brief untility member, all joint index collected into a std::vector, excluding the universe 195 | /// joint 196 | /// should be moved to Model 197 | /// 198 | IndexVector joint_range; 199 | 200 | /// 201 | /// \brief number of the task equality constraints 202 | /// 203 | int num_eq_c; 204 | 205 | /// 206 | /// \brief Ai.transpose * yi ; updated in the solver after dual variables (yis) udpate (i.e. in 207 | /// DualUpdate). 208 | /// this quantity is used store intermediate results 209 | /// for dual residual computation and primal infeasibility checking 210 | /// 211 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Aty; 212 | 213 | /// 214 | /// \brief record of Aty from previous iteration, needed for primal infeasibility checking 215 | /// 216 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Aty_prev; 217 | 218 | /// 219 | /// \brief \Delta f_i + A^T * y := - f_i + \sum_{j \in \gamma(i)} f_j + A^T * y. 220 | /// first updated in DualUpdate with A^T * y, then Updated in BwdPass2. 221 | /// Equivalent to top half of A_qp^T * y_qp 222 | /// this quantity is used to store intermediate results 223 | /// for dual residual computation and primal infeasibility checking 224 | /// 225 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) fis_diff_plus_Aty; 226 | 227 | /// 228 | /// \brief delta of fis_diff_plus_Aty from previous iteration, needed for primal infeasibility 229 | /// checking 230 | /// first copies the content of 'fis_diff_plus_Aty' (in DualUpdate) before it's updated. 231 | /// then compute the delta during BwdPass2 232 | /// 233 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) delta_fis_diff_plus_Aty; 234 | 235 | /// 236 | /// \brief H_ref * vi, , this quantity is used store intermediate results 237 | /// for dual residual computation and dual infeasibility checking. 238 | /// Updated in FwdPass2 239 | /// 240 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Href_v; 241 | 242 | /// 243 | /// \brief Ai * vi - bi, this quantity is used to store intermediate results 244 | /// for primal residual computation. Updated in DualUpdate 245 | /// 246 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) Av_minus_b; 247 | 248 | /// 249 | /// \brief Si.transpose() * fi + wi, this quantity is used store intermediate results 250 | /// for dual residual computation and primal infeasibility checking, updated in BwdPass2 251 | /// 252 | DVec Stf_plus_w; 253 | 254 | /// 255 | /// \brief record of Stf_plus_w from previous iteration, 256 | /// first creates an copy of Stf_plus_w from previous solver iter 257 | /// in BwdPass2 before going into the loop, then updated to be the 258 | /// difference after the loop. 259 | /// 260 | DVec delta_Stf_plus_w; 261 | 262 | /// 263 | /// \brief bi.transpose() * delta_y_plus, this quantity is used store intermediate results 264 | /// for primal infeasibility checking. Updated in DualUpdate 265 | /// 266 | Scalar bT_delta_y_plus; 267 | 268 | /// 269 | /// \brief bi.transpose() * delta_y_minus, used for primal infeasibility checking 270 | /// 271 | Scalar bT_delta_y_minus; 272 | 273 | /// 274 | /// \brief Av inf norm, updated in DualUpdate() 275 | /// 276 | Scalar Av_inf_norm; 277 | 278 | /// 279 | /// \brief nu inf norm, updated in FwdPass2() 280 | /// 281 | Scalar nu_inf_norm; 282 | 283 | /// 284 | /// \brief Href_v inf norm, updated in FwdPass2() 285 | /// 286 | Scalar Href_v_inf_norm; 287 | 288 | /// 289 | /// \brief fis_diff_plus_Aty inf norm, updated in BwdPass2() 290 | /// 291 | Scalar fis_diff_plus_Aty_inf_norm; 292 | 293 | /// 294 | /// \brief Stf_plus_w inf norm, updated in BwdPass2() 295 | /// 296 | Scalar Stf_plus_w_inf_norm; 297 | 298 | /// 299 | /// \brief 300 | /// 301 | Scalar delta_fis_diff_plus_Aty_inf_norm; 302 | 303 | /// 304 | /// \brief 305 | /// 306 | Scalar delta_Stf_plus_w_inf_norm; 307 | 308 | /// 309 | /// \brief computed in FwdPass2 310 | /// 311 | Scalar delta_vis_inf_norm; 312 | 313 | /// 314 | /// \brief computed in FwdPass2 315 | /// 316 | Scalar delta_nu_inf_norm; 317 | 318 | /// 319 | /// \brief computed in FwdPass2 320 | /// 321 | Scalar delta_z_inf_norm; 322 | 323 | /// 324 | /// \brief computed in BwdPass1 325 | /// 326 | Scalar delta_fis_inf_norm; 327 | 328 | /// 329 | /// \brief 330 | /// 331 | Scalar delta_yis_inf_norm; 332 | 333 | /// 334 | /// \brief 335 | /// 336 | Scalar delta_w_inf_norm; 337 | 338 | /// 339 | /// \brief default constructor 340 | /// 341 | IkIdDataTypeOptimizedTpl() {}; 342 | 343 | /// 344 | /// \brief construct from pinocchio::ModelTpl 345 | /// 346 | /// \param[in] model The model structure of the rigid body system. 347 | /// 348 | explicit IkIdDataTypeOptimizedTpl(const Model & model); 349 | 350 | /// 351 | /// \brief construct from pinocchio::ModelTpl and a task equality constraint dimension 352 | /// 353 | /// \param[in] model The model structure of the rigid body system. 354 | /// \param[in] num_eq_c Number of Task Equality constraints. 355 | /// 356 | explicit IkIdDataTypeOptimizedTpl(const Model & model, const int num_eq_c); 357 | 358 | /// 359 | /// \brief reset all quantities 360 | /// 361 | void Reset(const bool warm_start); 362 | 363 | /// 364 | /// \brief reset all recursion related quantities 365 | /// 366 | void ResetRecursion(); 367 | 368 | /// 369 | /// \brief reset all inf norm related quantities 370 | /// 371 | void ResetInfNorms(); 372 | 373 | /// 374 | /// \brief update _prev primal dual variables with that of current values 375 | /// 376 | void UpdatePrev(); 377 | 378 | }; // struct IkIdDataTypeOptimizedTpl 379 | 380 | /// 381 | /// \brief utility function for checking IkIdData against the Model it instantiated from 382 | /// 383 | template class JointCollectionTpl> 384 | inline bool checkIkIdData( 385 | const pinocchio::ModelTpl & model, 386 | const IkIdDataTypeOptimizedTpl & ik_id_data) 387 | { 388 | typedef pinocchio::ModelTpl Model; 389 | typedef IkIdDataTypeOptimizedTpl IkIdData; 390 | typedef typename Model::JointModel JointModel; 391 | 392 | std::cout << (static_cast(ik_id_data.joints.size()) == model.njoints) << std::endl; 393 | 394 | #define CHECK_DATA(a) \ 395 | if (!(a)) \ 396 | return false; 397 | 398 | // check sizes of each data members 399 | CHECK_DATA(static_cast(ik_id_data.joints.size()) == model.njoints); 400 | CHECK_DATA(static_cast(ik_id_data.oMi.size()) == model.njoints); 401 | CHECK_DATA(static_cast(ik_id_data.liMi.size()) == model.njoints); 402 | CHECK_DATA(static_cast(ik_id_data.nu.size()) == model.nv); 403 | CHECK_DATA(static_cast(ik_id_data.nu_prev.size()) == model.nv); 404 | CHECK_DATA(static_cast(ik_id_data.vis.size()) == model.njoints); 405 | CHECK_DATA(static_cast(ik_id_data.ovis.size()) == model.njoints); 406 | CHECK_DATA(static_cast(ik_id_data.vis_prev.size()) == model.njoints); 407 | CHECK_DATA(static_cast(ik_id_data.His_aba.size()) == model.njoints); 408 | CHECK_DATA(static_cast(ik_id_data.His.size()) == model.njoints); 409 | CHECK_DATA(static_cast(ik_id_data.pis.size()) == model.njoints); 410 | CHECK_DATA(static_cast(ik_id_data.pis_aba.size()) == model.njoints); 411 | CHECK_DATA(static_cast(ik_id_data.R.size()) == model.nv); 412 | CHECK_DATA(static_cast(ik_id_data.r.size()) == model.nv); 413 | CHECK_DATA(static_cast(ik_id_data.fis.size()) == model.njoints); 414 | CHECK_DATA(static_cast(ik_id_data.delta_fis.size()) == model.njoints); 415 | CHECK_DATA(static_cast(ik_id_data.yis.size()) == ik_id_data.num_eq_c); 416 | CHECK_DATA(static_cast(ik_id_data.delta_yis.size()) == ik_id_data.num_eq_c); 417 | CHECK_DATA(static_cast(ik_id_data.w.size()) == model.nv); 418 | CHECK_DATA(static_cast(ik_id_data.delta_w.size()) == model.nv); 419 | CHECK_DATA(static_cast(ik_id_data.z.size()) == model.nv); 420 | CHECK_DATA(static_cast(ik_id_data.z_prev.size()) == model.nv); 421 | CHECK_DATA(static_cast(ik_id_data.joint_full_range.size()) == model.njoints); 422 | CHECK_DATA(static_cast(ik_id_data.joint_range.size()) == model.njoints - 1); 423 | CHECK_DATA(static_cast(ik_id_data.Aty.size()) == ik_id_data.num_eq_c); 424 | CHECK_DATA(static_cast(ik_id_data.Aty_prev.size()) == ik_id_data.num_eq_c); 425 | CHECK_DATA(static_cast(ik_id_data.fis_diff_plus_Aty.size()) == model.njoints); 426 | CHECK_DATA(static_cast(ik_id_data.delta_fis_diff_plus_Aty.size()) == model.njoints); 427 | CHECK_DATA(static_cast(ik_id_data.Href_v.size()) == model.njoints); 428 | CHECK_DATA(static_cast(ik_id_data.Av_minus_b.size()) == ik_id_data.num_eq_c); 429 | CHECK_DATA(static_cast(ik_id_data.Stf_plus_w.size()) == model.nv); 430 | CHECK_DATA(static_cast(ik_id_data.delta_Stf_plus_w.size()) == model.nv); 431 | 432 | for (pinocchio::JointIndex joint_id = 1; 433 | joint_id < static_cast(model.njoints); ++joint_id) 434 | { 435 | const JointModel & jmodel = model.joints[joint_id]; 436 | 437 | CHECK_DATA(model.nqs[joint_id] == jmodel.nq()); 438 | CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q()); 439 | CHECK_DATA(model.nvs[joint_id] == jmodel.nv()); 440 | CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v()); 441 | } 442 | 443 | // check ik_id_data utility quantities 444 | typename IkIdData::IndexVector joint_full_range_test; 445 | joint_full_range_test.reserve(static_cast(model.njoints)); 446 | 447 | for (typename IkIdData::Index idx = 0; static_cast(idx) < model.njoints; idx++) 448 | { 449 | joint_full_range_test.push_back(idx); 450 | } 451 | 452 | typename IkIdData::IndexVector joint_range_test = joint_full_range_test; 453 | joint_range_test.erase(joint_range_test.begin()); 454 | 455 | CHECK_DATA(ik_id_data.joint_full_range == joint_full_range_test); 456 | CHECK_DATA(ik_id_data.joint_range == joint_range_test); 457 | 458 | #undef CHECK_DATA 459 | return true; 460 | } // checkIkIdData 461 | 462 | /// 463 | /// \brief utility function for checking IkIdData against pinocchio::Data 464 | /// 465 | template class JointCollectionTpl> 466 | inline bool checkIkIdData( 467 | const pinocchio::ModelTpl & model, 468 | const IkIdDataTypeOptimizedTpl & ik_id_data, 469 | const pinocchio::DataTpl & data) 470 | { 471 | 472 | bool size_dim_check = checkIkIdData(model, ik_id_data); 473 | 474 | if (size_dim_check) 475 | { 476 | #define CHECK_DATA(a) \ 477 | if (!(a)) \ 478 | return false; 479 | 480 | // check joint data created btw "ik_id_data" and "data" are the same 481 | for (std::size_t j = 1; j < static_cast(ik_id_data.joints.size()); ++j) 482 | { 483 | CHECK_DATA(ik_id_data.joints.at(j) == data.joints.at(j)); 484 | } 485 | 486 | #undef CHECK_DATA 487 | return true; 488 | } 489 | else 490 | { 491 | return false; 492 | } 493 | 494 | } // checkIkIdData 495 | 496 | } // namespace loik 497 | 498 | #include "loik/loik-loid-data-optimized.hxx" 499 | 500 | #if LOIK_ENABLE_TEMPLATE_INSTANTIATION 501 | #include "loik/loik-loid-data.txx" 502 | #endif // LOIK_ENABLE_TEMPLATE_INSTANTIATION 503 | -------------------------------------------------------------------------------- /include/loik/loik-loid.hxx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid.hpp" 8 | 9 | namespace loik 10 | { 11 | 12 | ///////////////////////////////////////////////////////////////////////////////////////////////// 13 | ///////////////////////////////////////////////////////////////////////////////////////////////// 14 | ///////////////////////////////////////////////////////////////////////////////////////////////// 15 | template 16 | void FirstOrderLoikTpl<_Scalar>::FwdPassInit(const DVec & q) 17 | { 18 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 19 | 20 | for (const auto & idx : joint_range_) 21 | { 22 | const JointModel & jmodel = model_.joints[idx]; 23 | JointData & jdata = ik_id_data_.joints[idx]; 24 | Index parent = model_.parents[idx]; 25 | 26 | // computes "M"s for each joint, i.e. displacement in current joint frame caused by 'self.q_' 27 | jmodel.calc(jdata, q); 28 | ik_id_data_.liMi[idx] = model_.jointPlacements[idx] * jdata.M(); 29 | ik_id_data_.oMi[idx] = ik_id_data_.oMi[parent] * ik_id_data_.liMi[idx]; 30 | } 31 | 32 | LOIK_EIGEN_MALLOC_ALLOWED(); 33 | } // FwdPassInit 34 | 35 | ///////////////////////////////////////////////////////////////////////////////////////////////// 36 | ///////////////////////////////////////////////////////////////////////////////////////////////// 37 | ///////////////////////////////////////////////////////////////////////////////////////////////// 38 | template 39 | void FirstOrderLoikTpl<_Scalar>::FwdPass1() 40 | { 41 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 42 | 43 | for (const auto & idx : joint_range_) 44 | { 45 | const JointModel & jmodel = model_.joints[idx]; 46 | 47 | int joint_nv = jmodel.nv(); 48 | int joint_idx_v = jmodel.idx_v(); 49 | 50 | // build Ris and ris TODO: Ris only need to be computed once for constant mu 51 | ik_id_data_.Ris[idx] = mu_ineq_ * DMat::Identity(joint_nv, joint_nv); 52 | const auto & wi = ik_id_data_.w.segment(joint_idx_v, joint_nv); 53 | const auto & zi = ik_id_data_.z.segment(joint_idx_v, joint_nv); 54 | ik_id_data_.ris[idx].noalias() = wi - mu_ineq_ * zi; 55 | 56 | const Mat6x6 & H_ref = problem_.H_refs_[idx]; 57 | const Motion & v_ref = problem_.v_refs_[idx]; 58 | ik_id_data_.His[idx].noalias() = this->rho_ * DMat::Identity(6, 6) + H_ref; 59 | 60 | ik_id_data_.pis[idx].noalias() = 61 | -this->rho_ * ik_id_data_.vis_prev[idx].toVector() - H_ref.transpose() * v_ref.toVector(); 62 | } 63 | 64 | Index c_vec_id = 0; 65 | for (const auto & c_id : problem_.active_task_constraint_ids_) 66 | { 67 | const Mat6x6 & Ai = problem_.Ais_[c_vec_id]; 68 | const Vec6 & bi = problem_.bis_[c_vec_id]; 69 | 70 | const DVec & yi = ik_id_data_.yis[c_id]; 71 | ik_id_data_.His[c_id].noalias() += mu_eq_ * Ai.transpose() * Ai; 72 | ik_id_data_.pis[c_id].noalias() += Ai.transpose() * yi - mu_eq_ * Ai.transpose() * bi; 73 | 74 | c_vec_id++; 75 | } 76 | 77 | LOIK_EIGEN_MALLOC_ALLOWED(); 78 | } // FwdPass1 79 | 80 | ///////////////////////////////////////////////////////////////////////////////////////////////// 81 | ///////////////////////////////////////////////////////////////////////////////////////////////// 82 | ///////////////////////////////////////////////////////////////////////////////////////////////// 83 | template 84 | void FirstOrderLoikTpl<_Scalar>::BwdPass() 85 | { 86 | 87 | // loop over joint range in reverse 88 | for (auto it = joint_range_.rbegin(); it != joint_range_.rend(); ++it) 89 | { 90 | Index idx = *it; 91 | 92 | JointData & jdata = ik_id_data_.joints[idx]; 93 | Index parent = model_.parents[idx]; 94 | 95 | const SE3 & liMi = ik_id_data_.liMi[idx]; 96 | const Mat6x6 & Hi = ik_id_data_.His[idx]; 97 | const Vec6 & pi = ik_id_data_.pis[idx]; 98 | const DMat & Si = jdata.S().matrix(); // TODO: this will cause memory allocation 99 | const DMat & Ri = ik_id_data_.Ris[idx]; 100 | const DVec & ri = ik_id_data_.ris[idx]; 101 | 102 | ik_id_data_.Dis[idx].noalias() = 103 | Ri + Si.transpose() * Hi * Si; // TODO: this will cause memory allocation 104 | ik_id_data_.Di_invs[idx].noalias() = 105 | ik_id_data_.Dis[idx].inverse(); // TODO: this will cause memory allocation 106 | const DMat & Di_inv = ik_id_data_.Di_invs[idx]; 107 | ik_id_data_.Pis[idx].noalias() = 108 | DMat::Identity(6, 6) 109 | - Hi * Si * Di_inv * Si.transpose(); // TODO: this will cause memory allocation 110 | const Mat6x6 & Pi = ik_id_data_.Pis[idx]; 111 | 112 | ik_id_data_.His[parent].noalias() += 113 | liMi.toDualActionMatrix() * (Pi * Hi) * liMi.toActionMatrixInverse(); 114 | ik_id_data_.pis[parent].noalias() += 115 | liMi.toDualActionMatrix() 116 | * (Pi * pi - Hi * Si * Di_inv * ri); // TODO: this will cause memory allocation 117 | } 118 | 119 | } // BwdPass 120 | 121 | ///////////////////////////////////////////////////////////////////////////////////////////////// 122 | ///////////////////////////////////////////////////////////////////////////////////////////////// 123 | ///////////////////////////////////////////////////////////////////////////////////////////////// 124 | template 125 | void FirstOrderLoikTpl<_Scalar>::FwdPass2() 126 | { 127 | 128 | for (const auto & idx : joint_range_) 129 | { 130 | 131 | const JointModel & jmodel = model_.joints[idx]; 132 | JointData & jdata = ik_id_data_.joints[idx]; 133 | Index parent = model_.parents[idx]; 134 | 135 | int joint_nv = jmodel.nv(); 136 | int joint_idx_v = jmodel.idx_v(); 137 | 138 | const DMat & Di_inv = ik_id_data_.Di_invs[idx]; 139 | const auto Si = jdata.S().matrix(); 140 | const Mat6x6 & Hi = ik_id_data_.His[idx]; 141 | const Vec6 & pi = ik_id_data_.pis[idx]; 142 | const DVec & ri = ik_id_data_.ris[idx]; 143 | const SE3 & liMi = ik_id_data_.liMi[idx]; 144 | const Motion vi_parent = liMi.actInv( 145 | ik_id_data_.vis[parent]); // ith joint's parent spatial velocity in joint i's local frame 146 | 147 | ik_id_data_.nu.segment(joint_idx_v, joint_nv).noalias() = 148 | -Di_inv * (Si.transpose() * (Hi * vi_parent.toVector() + pi) + ri); 149 | 150 | ik_id_data_.Si_nui_s[idx] = Motion(Si * ik_id_data_.nu.segment(joint_idx_v, joint_nv)); 151 | ik_id_data_.vis[idx] = vi_parent + ik_id_data_.Si_nui_s[idx]; 152 | 153 | ik_id_data_.fis[idx].noalias() = Hi * ik_id_data_.vis[idx].toVector() + pi; 154 | } 155 | 156 | } // FwdPass2 157 | 158 | ///////////////////////////////////////////////////////////////////////////////////////////////// 159 | ///////////////////////////////////////////////////////////////////////////////////////////////// 160 | ///////////////////////////////////////////////////////////////////////////////////////////////// 161 | template 162 | void FirstOrderLoikTpl<_Scalar>::BoxProj() 163 | { 164 | LOIK_EIGEN_MALLOC_NOT_ALLOWED(); 165 | // update slack 166 | ik_id_data_.z.noalias() = problem_.ub_.cwiseMin( 167 | problem_.lb_.cwiseMax(ik_id_data_.nu + (1.0 / mu_ineq_) * ik_id_data_.w)); 168 | LOIK_EIGEN_MALLOC_ALLOWED(); 169 | } // BoxProj 170 | 171 | ///////////////////////////////////////////////////////////////////////////////////////////////// 172 | ///////////////////////////////////////////////////////////////////////////////////////////////// 173 | ///////////////////////////////////////////////////////////////////////////////////////////////// 174 | template 175 | void FirstOrderLoikTpl<_Scalar>::DualUpdate() 176 | { 177 | 178 | // update dual variables associated with motion constraints 'ik_id_data_.yis' 179 | Index c_vec_id = 0; 180 | for (const auto & c_id : problem_.active_task_constraint_ids_) 181 | { 182 | const Mat6x6 & Ai = problem_.Ais_[c_vec_id]; 183 | const Vec6 & bi = problem_.bis_[c_vec_id]; 184 | const Motion & vi = ik_id_data_.vis[c_id]; 185 | 186 | ik_id_data_.yis[c_id].noalias() += mu_eq_ * (Ai * vi.toVector() - bi); 187 | 188 | c_vec_id++; 189 | } 190 | 191 | // update dual vairables associated with inequality slack induced equality constraints 192 | // 'ik_id_data_.w' 193 | ik_id_data_.w.noalias() += mu_ineq_ * (ik_id_data_.nu - ik_id_data_.z); 194 | 195 | } // DualUpdate 196 | 197 | ///////////////////////////////////////////////////////////////////////////////////////////////// 198 | ///////////////////////////////////////////////////////////////////////////////////////////////// 199 | ///////////////////////////////////////////////////////////////////////////////////////////////// 200 | template 201 | void FirstOrderLoikTpl<_Scalar>::UpdateQPADMMSolveLoopUtility() 202 | { 203 | // update standard qp formulation using primal dual variables from current iter 204 | problem_.UpdateQPADMMSolveLoop(ik_id_data_); 205 | } // UpdateQPADMMSolveLoopUtility 206 | 207 | ///////////////////////////////////////////////////////////////////////////////////////////////// 208 | ///////////////////////////////////////////////////////////////////////////////////////////////// 209 | ///////////////////////////////////////////////////////////////////////////////////////////////// 210 | template 211 | void FirstOrderLoikTpl<_Scalar>::ComputeResiduals() 212 | { 213 | 214 | const int m = problem_.eq_c_dim_; 215 | 216 | // compute primal residual 217 | Index c_vec_id = 0; 218 | for (const auto & c_id : problem_.active_task_constraint_ids_) 219 | { 220 | const Mat6x6 & Ai = problem_.Ais_[c_vec_id]; 221 | const Vec6 & bi = problem_.bis_[c_vec_id]; 222 | const Motion & vi = ik_id_data_.vis[c_id]; 223 | 224 | primal_residual_vec_.segment(m * (static_cast(c_id) - 1), m) = Ai * vi.toVector() - bi; 225 | 226 | c_vec_id++; 227 | } 228 | 229 | primal_residual_vec_.segment(m * nb_, nv_) = ik_id_data_.nu - ik_id_data_.z; 230 | this->primal_residual_ = primal_residual_vec_.template lpNorm(); 231 | primal_residual_task_ = 232 | primal_residual_vec_.segment(0, m * nb_).template lpNorm(); 233 | primal_residual_slack_ = 234 | primal_residual_vec_.segment(m * nb_, nv_).template lpNorm(); 235 | 236 | // compute dual residual 237 | for (auto it = joint_range_.rbegin(); it != joint_range_.rend(); ++it) 238 | { 239 | Index idx = *it; 240 | 241 | const Mat6x6 & H_ref = problem_.H_refs_[idx]; 242 | const Motion & v_ref = problem_.v_refs_[idx]; 243 | const Motion & vi = ik_id_data_.vis[idx]; 244 | const Vec6 & fi = ik_id_data_.fis[idx]; 245 | const SE3 & liMi = ik_id_data_.liMi[idx]; 246 | Index parent = model_.parents[idx]; 247 | 248 | int row_start_idx = (static_cast(idx) - 1) * 6; 249 | int row_start_idx_parent = 0; 250 | 251 | if (static_cast(parent) > 0) 252 | { 253 | row_start_idx_parent = (static_cast(parent) - 1) * 6; 254 | dual_residual_vec_.segment(row_start_idx, 6).noalias() += 255 | H_ref * vi.toVector() - H_ref * v_ref.toVector() - fi; 256 | dual_residual_vec_.segment(row_start_idx_parent, 6).noalias() += 257 | liMi.toDualActionMatrix() * fi; 258 | } 259 | else 260 | { 261 | row_start_idx_parent = 0; 262 | dual_residual_vec_.segment(row_start_idx, 6).noalias() += 263 | H_ref * vi.toVector() - H_ref * v_ref.toVector() - fi; 264 | } 265 | 266 | const JointModel & jmodel = model_.joints[idx]; 267 | const JointData & jdata = ik_id_data_.joints[idx]; 268 | const int row_start_idx_wi = jmodel.idx_v(); 269 | 270 | const auto & wi = ik_id_data_.w.segment(row_start_idx_wi, jmodel.nv()); 271 | const auto Si = jdata.S().matrix(); 272 | 273 | dual_residual_vec_.segment(6 * nb_ + row_start_idx_wi, jmodel.nv()).noalias() = 274 | Si.transpose() * fi + wi; 275 | } 276 | 277 | c_vec_id = 0; 278 | for (const auto & c_id : problem_.active_task_constraint_ids_) 279 | { 280 | const Mat6x6 & Ai = problem_.Ais_[c_vec_id]; 281 | const Vec6 & yi = ik_id_data_.yis[c_id]; 282 | 283 | int row_start_idx = (static_cast(c_id) - 1) * 6; 284 | 285 | dual_residual_vec_.segment(row_start_idx, 6).noalias() += Ai.transpose() * yi; 286 | 287 | c_vec_id++; 288 | } 289 | 290 | dual_residual_vec_ = problem_.P_qp_ * problem_.x_qp_ + problem_.q_qp_ 291 | + problem_.A_qp_.transpose() * problem_.y_qp_; 292 | 293 | dual_residual_prev_ = this->dual_residual_; 294 | dual_residual_v_prev_ = dual_residual_v_; 295 | dual_residual_nu_prev_ = dual_residual_nu_; 296 | 297 | this->dual_residual_ = dual_residual_vec_.template lpNorm(); 298 | dual_residual_v_ = (dual_residual_vec_.segment(0, 6 * nb_)).template lpNorm(); 299 | dual_residual_nu_ = 300 | (dual_residual_vec_.segment(6 * nb_, nv_)).template lpNorm(); 301 | 302 | delta_dual_residual_ = this->dual_residual_ - dual_residual_prev_; 303 | delta_dual_residual_v_ = dual_residual_v_ - dual_residual_v_prev_; 304 | delta_dual_residual_nu_ = dual_residual_nu_ - dual_residual_nu_prev_; 305 | 306 | } // ComputeResiduals 307 | 308 | ///////////////////////////////////////////////////////////////////////////////////////////////// 309 | ///////////////////////////////////////////////////////////////////////////////////////////////// 310 | ///////////////////////////////////////////////////////////////////////////////////////////////// 311 | template 312 | void FirstOrderLoikTpl<_Scalar>::CheckConvergence() 313 | { 314 | // update primal residual tolerance 315 | this->tol_primal_ = this->tol_abs_ 316 | + this->tol_rel_ 317 | * std::max( 318 | (problem_.A_qp_ * problem_.x_qp_).template lpNorm(), 319 | (problem_.z_qp_).template lpNorm()); 320 | 321 | // update dual residual tolerance 322 | this->tol_dual_ = 323 | this->tol_abs_ 324 | + this->tol_rel_ 325 | * std::max( 326 | std::max( 327 | (problem_.P_qp_ * problem_.x_qp_).template lpNorm(), 328 | (problem_.A_qp_.transpose() * problem_.y_qp_).template lpNorm()), 329 | (problem_.q_qp_).template lpNorm()); 330 | 331 | // check convergence 332 | if ((this->primal_residual_ < this->tol_primal_) && (this->dual_residual_ < this->tol_dual_)) 333 | { 334 | this->converged_ = true; 335 | 336 | if (this->verbose_) 337 | { 338 | std::cerr << "[FirstOrderLoik::CheckConvergence]: converged in " << this->iter_ 339 | << "iterations !!!" << std::endl; 340 | } 341 | } 342 | 343 | } // CheckConvergence 344 | 345 | ///////////////////////////////////////////////////////////////////////////////////////////////// 346 | ///////////////////////////////////////////////////////////////////////////////////////////////// 347 | ///////////////////////////////////////////////////////////////////////////////////////////////// 348 | template 349 | void FirstOrderLoikTpl<_Scalar>::CheckFeasibility() 350 | { 351 | 352 | // check for primal infeasibility 353 | bool primal_infeasibility_cond_1 = 354 | (problem_.A_qp_.transpose() * problem_.delta_y_qp_).template lpNorm() 355 | <= this->tol_primal_inf_ * (problem_.delta_y_qp_).template lpNorm(); 356 | 357 | bool primal_infeasibility_cond_2 = 358 | (problem_.ub_qp_.transpose() * problem_.delta_y_qp_plus_ 359 | + problem_.lb_qp_.transpose() * problem_.delta_y_qp_minus_) 360 | .value() 361 | <= this->tol_primal_inf_ * (problem_.delta_y_qp_).template lpNorm(); 362 | 363 | if (primal_infeasibility_cond_1 && primal_infeasibility_cond_2) 364 | { 365 | this->primal_infeasible_ = true; 366 | if (this->verbose_) 367 | { 368 | std::cerr 369 | << "WARNING [FirstOrderLoik::CheckFeasibility]: IK problem is primal infeasible !!!" 370 | << std::endl; 371 | } 372 | } 373 | 374 | // check for dual infeasibility 375 | bool dual_infeasibility_cond_1 = 376 | (problem_.P_qp_ * problem_.delta_x_qp_).template lpNorm() 377 | <= this->tol_dual_inf_ * (problem_.delta_x_qp_).template lpNorm(); 378 | bool dual_infeasibility_cond_2 = 379 | (problem_.q_qp_.transpose() * problem_.delta_x_qp_).value() 380 | <= this->tol_dual_inf_ * (problem_.delta_x_qp_).template lpNorm(); 381 | 382 | if (dual_infeasibility_cond_1 && dual_infeasibility_cond_2) 383 | { 384 | bool dual_infeasibility_cond_3 = 385 | ((problem_.A_qp_ * problem_.delta_x_qp_).array() 386 | >= -this->tol_dual_inf_ * (problem_.delta_x_qp_).template lpNorm()) 387 | .all(); 388 | bool dual_infeasibility_cond_4 = 389 | ((problem_.A_qp_ * problem_.delta_x_qp_).array() 390 | <= this->tol_dual_inf_ * (problem_.delta_x_qp_).template lpNorm()) 391 | .all(); 392 | 393 | if (dual_infeasibility_cond_3 && dual_infeasibility_cond_4) 394 | { 395 | this->dual_infeasible_ = true; 396 | if (this->verbose_) 397 | { 398 | std::cerr 399 | << "WARNING [FirstOrderLoik::CheckFeasibility]: IK problem is dual infeasible !!!" 400 | << std::endl; 401 | } 402 | } 403 | } 404 | } // CheckFeasibility 405 | 406 | ///////////////////////////////////////////////////////////////////////////////////////////////// 407 | ///////////////////////////////////////////////////////////////////////////////////////////////// 408 | ///////////////////////////////////////////////////////////////////////////////////////////////// 409 | template 410 | void FirstOrderLoikTpl<_Scalar>::UpdateMu() 411 | { 412 | if (this->mu_update_strat_ == ADMMPenaltyUpdateStrat::DEFAULT) 413 | { 414 | // update mu by threasholding primal and dual residual ratio 415 | if (this->primal_residual_ > 10 * this->dual_residual_) 416 | { 417 | this->mu_ *= 10; 418 | 419 | mu_eq_ = this->mu_equality_scale_factor_ * this->mu_; 420 | mu_ineq_ = this->mu_; 421 | return; 422 | } 423 | else if (this->dual_residual_ > 10 * this->primal_residual_) 424 | { 425 | this->mu_ *= 0.1; 426 | 427 | mu_eq_ = this->mu_equality_scale_factor_ * this->mu_; 428 | mu_ineq_ = this->mu_; 429 | return; 430 | } 431 | else 432 | { 433 | return; 434 | } 435 | } 436 | else if (this->mu_update_strat_ == ADMMPenaltyUpdateStrat::OSQP) 437 | { 438 | // using OSQP strategy 439 | throw(std::runtime_error( 440 | "[FirstOrderLoik::UpdateMu]: mu update strategy OSQP not yet implemented")); 441 | } 442 | else if (this->mu_update_strat_ == ADMMPenaltyUpdateStrat::MAXEIGENVALUE) 443 | { 444 | // use max eigen value strategy 445 | throw(std::runtime_error( 446 | "[FirstOrderLoik::UpdateMu]: mu update strategy MAXEIGENVALUE not yet implemented")); 447 | } 448 | else 449 | { 450 | throw(std::runtime_error("[FirstOrderLoik::UpdateMu]: mu update strategy not supported")); 451 | } 452 | } // UpdateMu 453 | 454 | } // namespace loik 455 | -------------------------------------------------------------------------------- /include/loik/ik-id-description.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/loik-loid-data.hpp" 8 | 9 | #include 10 | #include 11 | 12 | namespace loik 13 | { 14 | 15 | template 16 | struct IkProblemFormulation 17 | { 18 | using IkIdData = IkIdDataTpl<_Scalar>; 19 | using Motion = typename IkIdData::Motion; 20 | using Model = typename IkIdData::Model; 21 | using DMat = typename IkIdData::DMat; 22 | using DVec = typename IkIdData::DVec; 23 | using Vec3 = typename IkIdData::Vec3; 24 | using Vec6 = typename IkIdData::Vec6; 25 | using Mat6x6 = typename IkIdData::Mat6x6; 26 | using Index = typename IkIdData::Index; 27 | using IndexVec = typename IkIdData::IndexVector; 28 | 29 | explicit IkProblemFormulation( 30 | const int nj, const int nb, const int nc_eq, const int eq_c_dim, const int ineq_c_dim) 31 | : eq_c_dim_(eq_c_dim) 32 | , nj_(nj) 33 | , nb_(nb) 34 | , nc_eq_(nc_eq) 35 | , ineq_c_dim_(ineq_c_dim) 36 | { 37 | if (nj_ != nb_ + 1) 38 | { 39 | throw(std::runtime_error( 40 | "[IkProblemFormulation::IkProblemFormulation]: nb does not equal " 41 | "to nj - 1, robot model not supported !!!")); 42 | } 43 | 44 | if (eq_c_dim != 6) 45 | { 46 | throw(std::runtime_error( 47 | "[IkProblemFormulation::IkProblemFormulation]: equality constraint " 48 | "dimension is not 6, problem formulation not supported !!!")); 49 | } 50 | 51 | H_refs_.reserve(static_cast(nj)); 52 | v_refs_.reserve(static_cast(nj)); 53 | 54 | active_task_constraint_ids_.reserve(static_cast(nj)); 55 | Ais_.reserve(static_cast(nj)); 56 | bis_.reserve(static_cast(nj)); 57 | 58 | lb_ = Eigen::VectorXd::Zero(ineq_c_dim_); 59 | ub_ = Eigen::VectorXd::Zero(ineq_c_dim_); 60 | 61 | Reset(); 62 | }; 63 | 64 | void Reset() 65 | { 66 | 67 | // reset reference 68 | ResetReferences(); 69 | 70 | // reset equality constraints 71 | ResetEqConstraints(); 72 | 73 | // reset inequality constraints 74 | ResetIneqConstraints(); 75 | }; 76 | 77 | /// 78 | /// \brief set tracking reference by duplicating weights and targets for all body links 79 | /// 80 | void UpdateReference(const Mat6x6 & H_ref_target, const Motion & v_ref_target) 81 | { 82 | // check H_refs_ is not empty 83 | if (H_refs_.size() <= 0) 84 | { 85 | throw(std::runtime_error( 86 | "[IkProblemFormulation::UpdateReference]: 'H_refs_' and 'v_refs_' are empty")); 87 | } 88 | 89 | for (Index idx = 0; idx < H_refs_.size(); idx++) 90 | { 91 | H_refs_[idx] = H_ref_target; 92 | v_refs_[idx] = v_ref_target; 93 | } 94 | }; 95 | 96 | /// 97 | /// \brief set tracking references uisng vectors of references 98 | /// 99 | void UpdateReferences( 100 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & H_refs, 101 | const PINOCCHIO_ALIGNED_STD_VECTOR(Motion) & v_refs) 102 | { 103 | if ( 104 | H_refs.size() != static_cast(nj_) 105 | || v_refs.size() != static_cast(nj_)) 106 | { 107 | throw(std::runtime_error( 108 | "[IkProblemFormulation::UpdateReferences]: input arguments " 109 | "'H_refs', 'v_refs' have wrong size!!")); 110 | } 111 | H_refs_ = H_refs; 112 | v_refs_ = v_refs; 113 | }; 114 | 115 | /// 116 | /// \brief batch update equality constraints, number of equality constraints and constraint 117 | /// dimensions must not change 118 | /// 119 | void UpdateEqConstraints( 120 | const std::vector & active_task_constraint_ids, 121 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & Ais, 122 | const PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) & bis) 123 | { 124 | // check constraint specification is consistant 125 | if (!((active_task_constraint_ids.size() == Ais.size()) && ((Ais.size() == bis.size())))) 126 | { 127 | throw(std::runtime_error( 128 | "[IkProblemFormulation::UpdateEqConstraints]: " 129 | "task_constraint_ids, Ais, and bis have different size !!!")); 130 | } 131 | 132 | // check constraint dimension is consistent between input arguments! 133 | if (Ais[0].rows() != bis[0].rows()) 134 | { 135 | throw(std::runtime_error( 136 | "[IkProblemFormulation::UpdateEqConstraints] Ai and bi dimension mismatch!!")); 137 | } 138 | 139 | // check if number of equality constraints has changed 140 | if (active_task_constraint_ids.size() != static_cast(nc_eq_)) 141 | { 142 | throw(std::runtime_error( 143 | "[IkProblemFormulation::UpdateEqConstraints]: number of equality " 144 | "constraints doesn't match initialization!!!")); 145 | } 146 | 147 | // check if equality constraint dim has changed 148 | if (Ais[0].rows() != eq_c_dim_) 149 | { 150 | // TODO: need to check constraint dimension for each Ai and bi, not just the first ones 151 | throw(std::runtime_error( 152 | "[IkProblemFormulation::UpdateEqConstraints]: equality constraint " 153 | "dimension has changed!!! Updating constraint dimension")); 154 | } 155 | 156 | active_task_constraint_ids_ = active_task_constraint_ids; 157 | Ais_ = Ais; 158 | bis_ = bis; 159 | }; 160 | 161 | /// 162 | /// \brief set equality constraints by individual link id and updating 'Ai' and 'bi', the link 163 | /// id to update must already be 164 | /// present in 'active_task_constraint_ids_'. 165 | /// 166 | void UpdateEqConstraint(const Index c_id, const Mat6x6 & Ai, const Vec6 & bi) 167 | { 168 | // check if constraint already exist at link 'c_id' 169 | auto found_it = 170 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 171 | 172 | // if 'c_id' not present in 'active_task_constraint_ids_', invoke 'AddEqConstraint()' 173 | if (found_it == active_task_constraint_ids_.end()) 174 | { 175 | throw(std::runtime_error( 176 | "[IkProblemFormulation::UpdateEqConstraint]: constraint doesn't " 177 | "yet exist at link 'c_id' !!! ")); 178 | } 179 | 180 | // check if constraint dimension is consistent 181 | if (Ai.rows() != bi.size() || Ai.rows() != static_cast(eq_c_dim_)) 182 | { 183 | throw(std::runtime_error( 184 | "[IkProblemFormulation::UpdateEqConstraint]: constraint dimension inconsistent!!!")); 185 | } 186 | 187 | // count how many times 'c_id' appear in 'active_task_constraint_ids_' 188 | int c_id_count = 189 | std::count(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 190 | 191 | // if 'c_id' appear more than once, then something went wrong, throw. 192 | if (c_id_count > 1) 193 | { 194 | throw(std::runtime_error( 195 | "[IkProblemFormulation::UpdateEqConstraint]: multiple constraint " 196 | "specification for the same link id, not supported, terminating !!!")); 197 | } 198 | 199 | // get index of 'c_id' in 'active_task_constraint_ids_' 200 | Index c_id_in_vec = std::distance(active_task_constraint_ids_.begin(), found_it); 201 | 202 | // update Ai 203 | Ais_[c_id_in_vec] = Ai; 204 | // update bi 205 | bis_[c_id_in_vec] = bi; 206 | }; 207 | 208 | /// 209 | /// \brief update equality constriant by link id, update 'bi' only, keep using old 'Ai', 210 | /// constraint must already be present for link id 211 | /// 212 | void UpdateEqConstraint(const Index c_id, const Vec6 & bi) 213 | { 214 | // check if constraint already exist at link 'c_id' 215 | auto found_it = 216 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 217 | 218 | // if 'c_id' not present in 'active_task_constraint_ids_', then throw 219 | if (found_it == active_task_constraint_ids_.end()) 220 | { 221 | throw(std::runtime_error( 222 | "[IkProblemFormulation::UpdateEqConstraint]: constraint doesn't " 223 | "yet exist at link 'c_id' !!! ")); 224 | } 225 | 226 | // get index of 'c_id' in 'active_task_constraint_ids_' 227 | Index c_id_in_vec = std::distance(active_task_constraint_ids_.begin(), found_it); 228 | UpdateEqConstraint(c_id, Ais_[c_id_in_vec], bi); 229 | } 230 | 231 | /// 232 | /// \brief (deactivated for now) add equality constraint by link id. If constraint is already 233 | /// present for link id, then invoke 'UpdateEqConstraint()' 234 | /// 235 | void AddEqConstraint(const Index c_id, const Mat6x6 & Ai, const Vec6 & bi) 236 | { 237 | // check if 'c_id' in 'active_task_constraint_ids_' 238 | auto found_it = 239 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 240 | 241 | // if found, update instead 242 | if (found_it != active_task_constraint_ids_.end()) 243 | { 244 | // constraint already defined at link id, updating instead 245 | UpdateEqConstraint(c_id, Ai, bi); 246 | } 247 | else 248 | { 249 | // not found 250 | 251 | // check constraint dimension 252 | if (Ai.rows() != bi.rows()) 253 | { 254 | throw(std::runtime_error( 255 | "[IkProblemFormulation::AddEqConstraint]: input arguments " 256 | "constraint dimension inconsistent !!!")); 257 | } 258 | 259 | if (Ai.rows() != static_cast(eq_c_dim_)) 260 | { 261 | throw(std::runtime_error( 262 | "[IkProblemFormulation::AddEqConstraint]: input constraint " 263 | "dimension differ from existing constriant dimension!!!")); 264 | } 265 | 266 | active_task_constraint_ids_.push_back(c_id); 267 | nc_eq_++; 268 | 269 | // add Ai and bi 270 | Ais_.push_back(Ai); 271 | bis_.push_back(bi); 272 | } 273 | }; 274 | 275 | /// 276 | /// \brief (deactivated for now) remove equality constraint by link id, if constraint not 277 | /// already present for link id, then do nothing 278 | /// 279 | void RemoveEqConstraint(const Index c_id) 280 | { 281 | // check if constraint already exist at link 'c_id' 282 | auto found_it = 283 | std::find(active_task_constraint_ids_.begin(), active_task_constraint_ids_.end(), c_id); 284 | 285 | if (found_it == active_task_constraint_ids_.end()) 286 | { 287 | // no constraint defined at input link id, do nothing 288 | std::cerr << "WARNING [IkProblemFormulation::RemoveEqConstraint]: no constraint defined at " 289 | "link id, nothing to remove. " 290 | << std::endl; 291 | return; 292 | } 293 | 294 | // get index of 'c_id' in 'active_task_constraint_ids_' 295 | active_task_constraint_ids_.erase(found_it); 296 | Ais_.erase(found_it); 297 | bis_.erase(found_it); 298 | nc_eq_--; 299 | }; 300 | 301 | /// 302 | /// \brief set inequality constraints 303 | /// 304 | void UpdateIneqConstraints(const DVec & lb, const DVec & ub) 305 | { 306 | // check arguments have some dimension 307 | if (lb.size() != ub.size()) 308 | { 309 | throw(std::runtime_error( 310 | "[IkProblemFormulation::UpdateIneqConstraints]: lower bound and " 311 | "upper bound have different dimensions!!!")); 312 | } 313 | 314 | // check is inequality constraint dimension has changed 315 | if (lb.size() != ineq_c_dim_) 316 | { 317 | throw(std::runtime_error( 318 | "IkProblemFormulation::UpdateIneqConstraints]: inequality constraint " 319 | "dimension has changed, this is not supported currently!!!")); 320 | } 321 | 322 | lb_ = lb; 323 | ub_ = ub; 324 | }; 325 | 326 | int eq_c_dim_; // constraint dimension of the equality task constraint, this is shared across 327 | // all eq constraints 328 | int nj_; // number of joints 329 | int nb_; // number of bodies 330 | int nc_eq_; // number of equality constraints in the problem 331 | int ineq_c_dim_; // inequality constraint dimension, == pinocchio Model::nv 332 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) 333 | H_refs_; // weights for spatial velocity reference tracking in the cost, i.e. soft constraints 334 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v_refs_; // spatial velocity tracking references 335 | std::vector 336 | active_task_constraint_ids_; // vector of idx of links with active hard constraints 337 | 338 | PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) 339 | Ais_; // 'A' terms in the active equality constraint definitions 340 | PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) 341 | bis_; // 'b' terms in the active equality constraint definitions 342 | DVec lb_; // lower bound for primal decision variables 343 | DVec ub_; // upper bound for primal decision variables 344 | 345 | private: 346 | /// 347 | /// \brief reset tracking references weights and targets 348 | /// 349 | void ResetReferences() 350 | { 351 | H_refs_.clear(); 352 | v_refs_.clear(); 353 | H_refs_.resize(static_cast(nj_)); 354 | v_refs_.resize(static_cast(nj_)); 355 | for (Index idx = 0; idx < static_cast(nj_); idx++) 356 | { 357 | H_refs_[idx] = Mat6x6::Zero(); 358 | v_refs_[idx] = Motion::Zero(); 359 | } 360 | } 361 | 362 | /// 363 | /// \brief reset equality constraint quantities, 'Ais_' and 'bis_' 364 | /// 365 | void ResetEqConstraints() 366 | { 367 | active_task_constraint_ids_.clear(); 368 | Ais_.clear(); 369 | bis_.clear(); 370 | 371 | active_task_constraint_ids_.resize(static_cast(nc_eq_)); 372 | Ais_.resize(static_cast(nc_eq_)); 373 | bis_.resize(static_cast(nc_eq_)); 374 | 375 | for (Index idx = 0; idx < static_cast(nc_eq_); idx++) 376 | { 377 | active_task_constraint_ids_[idx] = 0; 378 | Ais_[idx] = Mat6x6::Zero(); 379 | bis_[idx] = Vec6::Zero(); 380 | } 381 | } 382 | 383 | /// 384 | /// \brief reset inequality constriant quantities, 'lb_' and 'ub_' 385 | /// 386 | void ResetIneqConstraints() 387 | { 388 | lb_.setZero(); 389 | ub_.setZero(); 390 | } 391 | 392 | }; // struct IkProblemFormulation 393 | 394 | template 395 | struct IkProblemStandardQPFormulation : IkProblemFormulation<_Scalar> 396 | { 397 | using Base = IkProblemFormulation<_Scalar>; 398 | using IkIdData = typename Base::IkIdData; 399 | using Model = typename Base::Model; 400 | using Motion = typename Base::Motion; 401 | using DMat = typename Base::DMat; 402 | using DVec = typename Base::DVec; 403 | using Vec3 = typename Base::Vec3; 404 | using Vec6 = typename Base::Vec6; 405 | using Mat6x6 = typename Base::Mat6x6; 406 | using Index = typename Base::Index; 407 | using IndexVec = typename Base::IndexVec; 408 | 409 | explicit IkProblemStandardQPFormulation( 410 | const int nj, const int nb, const int nc_eq, const int eq_c_dim, const int ineq_c_dim) 411 | : Base(nj, nb, nc_eq, eq_c_dim, ineq_c_dim) 412 | { 413 | qp_constraint_dim_ = 6 * this->nb_ + this->eq_c_dim_ * this->nb_ + this->ineq_c_dim_; 414 | qp_var_dim_ = 6 * this->nb_ + this->ineq_c_dim_; 415 | 416 | Reset(); 417 | } 418 | 419 | /// 420 | /// \brief this should only be called to completely reset the problem formulation, it will set 421 | /// all quantities to zero, 422 | /// but problem dimensions and number of bodies and constraints won't change 423 | /// 424 | void Reset() 425 | { 426 | Base::Reset(); 427 | // hard constraints (equality) 428 | A_qp_ = DMat::Zero(qp_constraint_dim_, qp_var_dim_); 429 | x_qp_ = DVec::Zero(qp_var_dim_); 430 | z_qp_ = DVec::Zero(qp_constraint_dim_); 431 | y_qp_ = DVec::Zero(qp_constraint_dim_); 432 | 433 | // soft constraints (cost) 434 | P_qp_ = DMat::Zero(qp_var_dim_, qp_var_dim_); 435 | q_qp_ = DVec::Zero(qp_var_dim_); 436 | 437 | // hard constraints (slack inequality) 438 | lb_qp_ = DVec::Zero(qp_constraint_dim_); 439 | ub_qp_ = DVec::Zero(qp_constraint_dim_); 440 | 441 | // 442 | x_qp_prev_ = DVec::Zero(qp_var_dim_); 443 | z_qp_prev_ = DVec::Zero(qp_constraint_dim_); 444 | y_qp_prev_ = DVec::Zero(qp_constraint_dim_); 445 | 446 | // 447 | delta_x_qp_ = DVec::Zero(qp_var_dim_); 448 | delta_z_qp_ = DVec::Zero(qp_constraint_dim_); 449 | delta_y_qp_ = DVec::Zero(qp_constraint_dim_); 450 | 451 | // 452 | delta_y_qp_plus_ = DVec::Zero(qp_constraint_dim_); 453 | delta_y_qp_minus_ = DVec::Zero(qp_constraint_dim_); 454 | }; 455 | 456 | /// 457 | /// \brief problem formulation update function, should be called before runing ADMM iterations 458 | /// should only be called once per ADMM solve() call, used to update equality constraints 459 | /// and tracking references 460 | /// 461 | void UpdateQPADMMSolveInit( 462 | const Mat6x6 & H_ref, 463 | const Motion & v_ref, 464 | const std::vector & active_task_constraint_ids, 465 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & Ais, 466 | const PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) & bis, 467 | const DVec & lb, 468 | const DVec & ub, 469 | const Model & model, 470 | const IkIdData & ik_id_data) 471 | { 472 | IndexVec joint_full_range = ik_id_data.joint_full_range; 473 | IndexVec joint_range = ik_id_data.joint_range; 474 | 475 | Base::UpdateReference(H_ref, v_ref); 476 | Base::UpdateIneqConstraints(lb, ub); 477 | Base::UpdateEqConstraints(active_task_constraint_ids, Ais, bis); 478 | 479 | lb_qp_.segment(6 * this->nb_ + this->eq_c_dim_ * this->nb_, this->ineq_c_dim_) = this->lb_; 480 | ub_qp_.segment(6 * this->nb_ + this->eq_c_dim_ * this->nb_, this->ineq_c_dim_) = this->ub_; 481 | 482 | A_qp_.block(0, 0, 6 * this->nb_, 6 * this->nb_) = 483 | -1.0 * DMat::Identity(6 * this->nb_, 6 * this->nb_); 484 | A_qp_.block( 485 | 6 * this->nb_ + this->eq_c_dim_ * this->nb_, 6 * this->nb_, this->ineq_c_dim_, 486 | this->ineq_c_dim_) = DMat::Identity(this->ineq_c_dim_, this->ineq_c_dim_); 487 | 488 | // build A_qp, P_qp, q_qp 489 | for (Index idx : joint_range) 490 | { 491 | 492 | // fill in the Hi_ref and vi_ref, into P_qp_ and q_qp_ 493 | int row_start_idx_Hi_vi = (static_cast(idx) - 1) * 6; 494 | int col_start_idx_Hi_vi = (static_cast(idx) - 1) * 6; 495 | 496 | P_qp_.block(row_start_idx_Hi_vi, col_start_idx_Hi_vi, 6, 6).noalias() = this->H_refs_[idx]; 497 | q_qp_.segment(row_start_idx_Hi_vi, 6).noalias() = 498 | -(this->H_refs_[idx]).transpose() * (this->v_refs_[idx]).toVector(); 499 | 500 | // fill in the Sis into A_qp_ 501 | int row_start_idx_Si = (static_cast(idx) - 1) * 6; 502 | int joint_nv = model.joints[idx].nv(); 503 | int col_start_idx_Si = 6 * this->nb_ + model.joints[idx].idx_v(); 504 | A_qp_.block(row_start_idx_Si, col_start_idx_Si, 6, joint_nv).noalias() = 505 | ik_id_data.joints[idx].S().matrix(); 506 | 507 | // fill in the band diagonal frame transformations into A_qp_ 508 | int col_start_idx_i = (static_cast(idx) - 1) * 6; 509 | Index parent = model.parents[idx]; 510 | // ik_id_data.oMi[idx].inverse().toActionMatrix(); //iMo 511 | 512 | if (parent > 0) 513 | { 514 | // ik_id_data.oMi[parent].toActionMatrix(); // oMp 515 | int col_start_idx_parent = (static_cast(parent) - 1) * 6; 516 | 517 | // |<------------------ 518 | // iMo 519 | // ------------------>| 520 | // |<--------------- 521 | // oMp 522 | // --------------->| 523 | A_qp_.block(row_start_idx_Si, col_start_idx_parent, 6, 6).noalias() = 524 | ik_id_data.oMi[idx].inverse().toActionMatrix() 525 | * ik_id_data.oMi[parent].toActionMatrix(); 526 | A_qp_.block(row_start_idx_Si, col_start_idx_i, 6, 6).noalias() = -Mat6x6::Identity(); 527 | } 528 | else if (parent == 0) 529 | { 530 | A_qp_.block(row_start_idx_Si, col_start_idx_i, 6, 6).noalias() = -Mat6x6::Identity(); 531 | } 532 | else 533 | { 534 | throw(std::runtime_error( 535 | "[IkProblemStandardQPFormulation::UpdateQPADMMSolveInit]: " 536 | "parent id < 0, this can't happen !!!")); 537 | } 538 | } 539 | 540 | // build lb_qp, ub_qp 541 | Index c_idx_in_vec = 0; 542 | for (Index c_idx : this->active_task_constraint_ids_) 543 | { 544 | 545 | // fill in the Ais from motion constraints into A_qp_ 546 | 547 | int row_start_idx_Ai = 6 * this->nb_ + (static_cast(c_idx) - 1) * this->eq_c_dim_; 548 | int col_start_idx_Ai = (static_cast(c_idx) - 1) * 6; 549 | 550 | A_qp_.block(row_start_idx_Ai, col_start_idx_Ai, this->eq_c_dim_, 6).noalias() = 551 | this->Ais_[c_idx_in_vec]; 552 | 553 | // fill in the bis from motion constraints into ub_qp_ and lb_qp_ 554 | int row_start_idx_boundi = 6 * this->nb_ + (static_cast(c_idx) - 1) * this->eq_c_dim_; 555 | 556 | lb_qp_.segment(row_start_idx_boundi, this->eq_c_dim_).noalias() = this->bis_[c_idx_in_vec]; 557 | ub_qp_.segment(row_start_idx_boundi, this->eq_c_dim_).noalias() = this->bis_[c_idx_in_vec]; 558 | 559 | c_idx_in_vec++; 560 | } 561 | 562 | // fill part of z_qp_ for motion constriants 563 | z_qp_.segment(6 * this->nb_, this->eq_c_dim_ * this->nb_).noalias() = 564 | ub_qp_.segment(6 * this->nb_, this->eq_c_dim_ * this->nb_); 565 | }; 566 | 567 | /// 568 | /// \brief update x_qp_, y_qp_, and z_qp_ after each ADMM solve, 569 | /// 570 | void UpdateQPADMMSolveLoop(const IkIdData & ik_id_data) 571 | { 572 | // update, log quantities from previous iterations 573 | x_qp_prev_ = x_qp_; 574 | z_qp_prev_ = z_qp_; 575 | y_qp_prev_ = y_qp_; 576 | 577 | for (Index idx : ik_id_data.joint_range) 578 | { 579 | int row_start_idx_vi = (static_cast(idx) - 1) * 6; 580 | int row_start_idx_fi = (static_cast(idx) - 1) * 6; 581 | int row_start_idx_yi = 6 * this->nb_ + (static_cast(idx) - 1) * this->eq_c_dim_; 582 | 583 | // fill x_qp_ 584 | x_qp_.segment(row_start_idx_vi, 6).noalias() = ik_id_data.vis[idx].toVector(); // vi 585 | 586 | // fill dual variable y_qp associated with all constraints 587 | y_qp_.segment(row_start_idx_fi, 6).noalias() = ik_id_data.fis[idx]; // fi 588 | y_qp_.segment(row_start_idx_yi, this->eq_c_dim_).noalias() = ik_id_data.yis[idx]; // yi 589 | } 590 | 591 | // fill remainder of x_qp_ 592 | x_qp_.segment(6 * this->nb_, this->ineq_c_dim_).noalias() = ik_id_data.nu; 593 | 594 | // fill remainder of y_qp_ 595 | y_qp_.segment(6 * this->nb_ + this->nb_ * this->eq_c_dim_, this->ineq_c_dim_).noalias() = 596 | ik_id_data.w; 597 | 598 | // fill part of z_qp_ related to inequality slack 599 | z_qp_.segment(6 * this->nb_ + this->eq_c_dim_ * this->nb_, this->ineq_c_dim_).noalias() = 600 | ik_id_data.z; 601 | 602 | // udpate deltas 603 | delta_x_qp_ = x_qp_ - x_qp_prev_; 604 | delta_y_qp_ = y_qp_ - y_qp_prev_; 605 | delta_z_qp_ = z_qp_ - z_qp_prev_; 606 | 607 | // update delta_y_qp_ plus and minus 608 | delta_y_qp_plus_ = delta_y_qp_.cwiseMax(0); 609 | delta_y_qp_minus_ = delta_y_qp_.cwiseMin(0); 610 | }; 611 | 612 | int qp_constraint_dim_; 613 | int qp_var_dim_; 614 | DMat A_qp_; // constraint matrix 'A' in OSQP problem formulation 615 | DVec x_qp_; // primal decision variables in OSQP problem formulation 616 | DVec z_qp_; // slack variables in OSQP problem formulation 617 | DVec y_qp_; // dual variables in OSQP problem formulation 618 | DMat P_qp_; // quadratic cost weight in OSQP problem formulation 619 | DVec q_qp_; // linear cost in OSQP problem formulation 620 | DVec lb_qp_; // lower bound for primal variables in OSQP problem formulation 621 | DVec ub_qp_; // upper bound for primal variables in OSQP problem formulation 622 | 623 | DVec x_qp_prev_; 624 | DVec z_qp_prev_; 625 | DVec y_qp_prev_; 626 | 627 | DVec delta_x_qp_; 628 | DVec delta_z_qp_; 629 | DVec delta_y_qp_; 630 | 631 | DVec delta_y_qp_plus_; // copy of 'delta_y_qp_' with all negative coefficients set to 0 632 | DVec delta_y_qp_minus_; // copy of 'delta_y_qp_' with all positive coefficients set to 0 633 | 634 | }; // struct IkProblemStandardQPFormulation 635 | 636 | } // namespace loik 637 | -------------------------------------------------------------------------------- /include/loik/loik-loid.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #pragma once 6 | 7 | #include "loik/macros.hpp" 8 | #include "loik/ik-id-description.hpp" 9 | #include "loik/loik-loid-data.hpp" 10 | #include "loik/task-solver-base.hpp" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace loik 18 | { 19 | 20 | template 21 | struct FirstOrderLoikTpl : IkIdSolverBaseTpl<_Scalar> 22 | { 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | typedef IkIdSolverBaseTpl<_Scalar> Base; 26 | typedef typename Base::Scalar Scalar; 27 | using Model = pinocchio::ModelTpl<_Scalar>; 28 | using IkIdData = IkIdDataTpl<_Scalar>; 29 | using JointModel = typename IkIdData::JointModel; 30 | using JointData = typename IkIdData::JointData; 31 | using ProblemFormulation = IkProblemStandardQPFormulation<_Scalar>; 32 | using Motion = typename IkIdData::Motion; 33 | using SE3 = typename IkIdData::SE3; 34 | using DMat = typename IkIdData::DMat; 35 | using DVec = typename IkIdData::DVec; 36 | using Vec3 = typename IkIdData::Vec3; 37 | using Vec6 = typename IkIdData::Vec6; 38 | using Mat6x6 = typename IkIdData::Mat6x6; 39 | using Index = typename IkIdData::Index; 40 | using IndexVec = typename IkIdData::IndexVector; 41 | 42 | struct LoikSolverInfo : Base::SolverInfo 43 | { 44 | explicit LoikSolverInfo(const int max_iter) 45 | : Base::SolverInfo(max_iter) 46 | { 47 | primal_residual_kinematics_list_.reserve(static_cast(max_iter)); 48 | primal_residual_task_list_.reserve(static_cast(max_iter)); 49 | primal_residual_slack_list_.reserve(static_cast(max_iter)); 50 | 51 | dual_residual_v_list_.reserve(static_cast(max_iter)); 52 | dual_residual_nu_list_.reserve(static_cast(max_iter)); 53 | 54 | mu_eq_list_.reserve(static_cast(max_iter)); 55 | mu_ineq_list_.reserve(static_cast(max_iter)); 56 | 57 | tail_solve_iter_list_.reserve(static_cast(max_iter)); 58 | tail_solve_primal_residual_list_.reserve(static_cast(max_iter)); 59 | tail_solve_primal_residual_kinematics_list_.reserve(static_cast(max_iter)); 60 | tail_solve_primal_residual_task_list_.reserve(static_cast(max_iter)); 61 | tail_solve_primal_residual_slack_list_.reserve(static_cast(max_iter)); 62 | 63 | tail_solve_dual_residual_list_.reserve(static_cast(max_iter)); 64 | tail_solve_dual_residual_v_list_.reserve(static_cast(max_iter)); 65 | tail_solve_dual_residual_nu_list_.reserve(static_cast(max_iter)); 66 | 67 | tail_solve_delta_x_qp_inf_norm_list_.reserve(static_cast(max_iter)); 68 | tail_solve_delta_z_qp_inf_norm_list_.reserve(static_cast(max_iter)); 69 | }; 70 | 71 | void Reset() 72 | { 73 | Base::SolverInfo::Reset(); 74 | primal_residual_kinematics_list_.clear(); 75 | primal_residual_task_list_.clear(); 76 | primal_residual_slack_list_.clear(); 77 | 78 | dual_residual_v_list_.clear(); 79 | dual_residual_nu_list_.clear(); 80 | 81 | mu_eq_list_.clear(); 82 | mu_ineq_list_.clear(); 83 | 84 | tail_solve_iter_list_.clear(); 85 | tail_solve_primal_residual_list_.clear(); 86 | tail_solve_primal_residual_kinematics_list_.clear(); 87 | tail_solve_primal_residual_task_list_.clear(); 88 | tail_solve_primal_residual_slack_list_.clear(); 89 | 90 | tail_solve_dual_residual_list_.clear(); 91 | tail_solve_dual_residual_v_list_.clear(); 92 | tail_solve_dual_residual_nu_list_.clear(); 93 | 94 | tail_solve_delta_x_qp_inf_norm_list_.clear(); 95 | tail_solve_delta_z_qp_inf_norm_list_.clear(); 96 | } 97 | 98 | std::vector primal_residual_kinematics_list_; 99 | std::vector primal_residual_task_list_; 100 | std::vector primal_residual_slack_list_; 101 | 102 | std::vector dual_residual_v_list_; 103 | std::vector dual_residual_nu_list_; 104 | 105 | std::vector mu_eq_list_; 106 | std::vector mu_ineq_list_; 107 | 108 | // for tail solve iterations 109 | std::vector tail_solve_iter_list_; 110 | std::vector tail_solve_primal_residual_list_; 111 | std::vector tail_solve_primal_residual_kinematics_list_; 112 | std::vector tail_solve_primal_residual_task_list_; 113 | std::vector tail_solve_primal_residual_slack_list_; 114 | 115 | std::vector tail_solve_dual_residual_list_; 116 | std::vector tail_solve_dual_residual_v_list_; 117 | std::vector tail_solve_dual_residual_nu_list_; 118 | 119 | std::vector tail_solve_delta_x_qp_inf_norm_list_; 120 | std::vector tail_solve_delta_z_qp_inf_norm_list_; 121 | 122 | // TODO: maybe record the dual variables 123 | }; 124 | 125 | FirstOrderLoikTpl( 126 | const int max_iter, 127 | const Scalar & tol_abs, 128 | const Scalar & tol_rel, 129 | const Scalar & tol_primal_inf, 130 | const Scalar & tol_dual_inf, 131 | const Scalar & rho, 132 | const Scalar & mu, 133 | const Scalar & mu_equality_scale_factor, 134 | const ADMMPenaltyUpdateStrat & mu_update_strat, 135 | const int num_eq_c, 136 | const int eq_c_dim, 137 | const Model & model, 138 | IkIdData & ik_id_data, 139 | const bool warm_start, 140 | const Scalar tol_tail_solve, 141 | const bool verbose, 142 | const bool logging) 143 | : Base( 144 | max_iter, 145 | tol_abs, 146 | tol_rel, 147 | tol_primal_inf, 148 | tol_dual_inf, 149 | rho, 150 | mu, 151 | mu_equality_scale_factor, 152 | mu_update_strat, 153 | verbose, 154 | logging) 155 | , model_(model) 156 | , ik_id_data_(ik_id_data) 157 | , problem_(model.njoints, model.njoints - 1, num_eq_c, eq_c_dim, model.nv) 158 | , nj_(model.njoints) 159 | , nb_(model.njoints - 1) 160 | , nv_(model.nv) 161 | , warm_start_(warm_start) 162 | , tol_tail_solve_(tol_tail_solve) 163 | , loik_solver_info_(max_iter) 164 | { 165 | // initialize helper quantities 166 | joint_full_range_ = ik_id_data_.joint_full_range; // [0, nj - 1] 167 | joint_range_ = ik_id_data_.joint_range; // [1, nj - 1] 168 | 169 | ResetSolver(); 170 | }; 171 | 172 | /// 173 | /// \brief Reset the diff IK solver 174 | /// 175 | void ResetSolver() 176 | { 177 | loik_solver_info_.Reset(); // reset logging 178 | problem_.Reset(); // reset problem formulation 179 | Base::Reset(); // reset base 180 | 181 | ik_id_data_.Reset(this->warm_start_); // reset IkIdData 182 | 183 | tail_solve_iter_ = 0; 184 | 185 | primal_residual_kinematics_ = std::numeric_limits::infinity(); 186 | primal_residual_task_ = std::numeric_limits::infinity(); 187 | primal_residual_slack_ = std::numeric_limits::infinity(); 188 | 189 | dual_residual_prev_ = std::numeric_limits::infinity(); 190 | delta_dual_residual_ = std::numeric_limits::infinity(); 191 | dual_residual_v_ = std::numeric_limits::infinity(); 192 | ; 193 | dual_residual_v_prev_ = std::numeric_limits::infinity(); 194 | delta_dual_residual_v_ = std::numeric_limits::infinity(); 195 | dual_residual_nu_ = std::numeric_limits::infinity(); 196 | dual_residual_nu_prev_ = std::numeric_limits::infinity(); 197 | delta_dual_residual_nu_ = std::numeric_limits::infinity(); 198 | 199 | primal_residual_vec_ = 0.0 * DVec::Ones(problem_.eq_c_dim_ * nb_ + nv_); 200 | dual_residual_vec_ = 0.0 * DVec::Ones(6 * nb_ + nv_); 201 | 202 | mu_eq_ = this->mu_equality_scale_factor_ 203 | * this->mu_; // 'mu_' is reset to 'mu0_' during 'Base::Reset()' 204 | mu_ineq_ = this->mu_; 205 | 206 | }; // ResetSolver 207 | 208 | /// 209 | /// \brief Initial forward pass, to propagate forward kinematics. 210 | /// 211 | void FwdPassInit(const DVec & q); 212 | 213 | /// 214 | /// \brief LOIK first forward pass 215 | /// 216 | void FwdPass1(); 217 | 218 | /// 219 | /// \brief LOIK first packward pass 220 | /// 221 | void BwdPass(); 222 | 223 | /// 224 | /// \brief LOIK second forward pass 225 | /// 226 | void FwdPass2(); 227 | 228 | /// 229 | /// \brief Box projection of primal and slack composite quantites 230 | /// 231 | void BoxProj(); 232 | 233 | /// 234 | /// \brief ADMM dual variable updates 235 | /// 236 | void DualUpdate(); 237 | 238 | /// 239 | /// \brief unit test utility function 240 | /// 241 | void UpdateQPADMMSolveLoopUtility(); 242 | 243 | /// 244 | /// \brief Compute solver residuals 245 | /// 246 | void ComputeResiduals(); 247 | 248 | /// 249 | /// \brief Check primal and dual convergence 250 | /// 251 | void CheckConvergence(); 252 | 253 | /// 254 | /// \brief Check primal and dual feasibility 255 | /// 256 | void CheckFeasibility(); 257 | 258 | /// 259 | /// \brief Update ADMM penalty mu 260 | /// 261 | void UpdateMu(); 262 | 263 | /// 264 | /// \brief when infeasibility is detected, run tail solve so primal residual converges to 265 | /// something. 266 | /// This gives theoretical guarantee that the solution (unprojected) converges the 267 | /// closest feasible solution. 268 | /// 269 | void InfeasibilityTailSolve() 270 | { 271 | tail_solve_iter_ = 0; 272 | 273 | while (problem_.delta_x_qp_.template lpNorm() >= tol_tail_solve_ 274 | || problem_.delta_z_qp_.template lpNorm() >= tol_tail_solve_) 275 | { 276 | 277 | if (this->iter_ >= this->max_iter_) 278 | { 279 | if (this->verbose_) 280 | { 281 | std::cerr 282 | << "WARNING [FirstOrderLoik::InfeasibilityTailSolve]: tail solve exceeds max_iter_: " 283 | << tail_solve_iter_ << " iterations." << std::endl; 284 | std::cerr << "[FirstOrderLoik::InfeasibilityTailSolve]: normInf delta_x_qp_: " 285 | << problem_.delta_x_qp_.template lpNorm() << std::endl; 286 | std::cerr << "[FirstOrderLoik::InfeasibilityTailSolve]: normInf delta_z_qp_: " 287 | << problem_.delta_z_qp_.template lpNorm() << std::endl; 288 | } 289 | return; 290 | } 291 | 292 | this->iter_++; 293 | 294 | if (this->verbose_) 295 | { 296 | std::cout << "===============" << std::endl; 297 | std::cout << "ADMM iter: " << this->iter_ << "||" << std::endl; 298 | std::cout << "===============" << std::endl; 299 | } 300 | 301 | loik_solver_info_.iter_list_.push_back(this->iter_); 302 | 303 | tail_solve_iter_++; 304 | loik_solver_info_.tail_solve_iter_list_.push_back(tail_solve_iter_); 305 | 306 | ik_id_data_.UpdatePrev(); 307 | 308 | // fwd pass 1 309 | FwdPass1(); 310 | 311 | // bwd pass 312 | BwdPass(); 313 | 314 | // fwd pass 2 315 | FwdPass2(); 316 | 317 | // box projection 318 | BoxProj(); 319 | 320 | // dual update 321 | DualUpdate(); 322 | 323 | // update standard qp formulation using primal dual variables from current iter 324 | problem_.UpdateQPADMMSolveLoop(ik_id_data_); 325 | 326 | // compute residuals 327 | ComputeResiduals(); 328 | 329 | if (this->logging_) 330 | { 331 | 332 | // logging residuals TODO: should be disabled for speed 333 | loik_solver_info_.primal_residual_task_list_.push_back(primal_residual_task_); 334 | loik_solver_info_.primal_residual_slack_list_.push_back(primal_residual_slack_); 335 | loik_solver_info_.primal_residual_list_.push_back(this->primal_residual_); 336 | loik_solver_info_.dual_residual_nu_list_.push_back(dual_residual_nu_); 337 | loik_solver_info_.dual_residual_v_list_.push_back(dual_residual_v_); 338 | loik_solver_info_.dual_residual_list_.push_back(this->dual_residual_); 339 | 340 | loik_solver_info_.mu_list_.push_back(this->mu_); 341 | loik_solver_info_.mu_eq_list_.push_back(mu_eq_); 342 | loik_solver_info_.mu_ineq_list_.push_back(mu_ineq_); 343 | 344 | loik_solver_info_.tail_solve_primal_residual_task_list_.push_back(primal_residual_task_); 345 | loik_solver_info_.tail_solve_primal_residual_slack_list_.push_back( 346 | primal_residual_slack_); 347 | loik_solver_info_.tail_solve_primal_residual_list_.push_back(this->primal_residual_); 348 | loik_solver_info_.tail_solve_dual_residual_nu_list_.push_back(dual_residual_nu_); 349 | loik_solver_info_.tail_solve_dual_residual_v_list_.push_back(dual_residual_v_); 350 | loik_solver_info_.tail_solve_dual_residual_list_.push_back(this->dual_residual_); 351 | loik_solver_info_.tail_solve_delta_x_qp_inf_norm_list_.push_back( 352 | problem_.delta_x_qp_.template lpNorm()); 353 | loik_solver_info_.tail_solve_delta_z_qp_inf_norm_list_.push_back( 354 | problem_.delta_z_qp_.template lpNorm()); 355 | } 356 | } 357 | 358 | if (this->verbose_) 359 | { 360 | std::cerr << "[FirstOrderLoik::InfeasibilityTailSolve]: tail solve completed after " 361 | << tail_solve_iter_ << " iterations." << std::endl; 362 | std::cerr << "[FirstOrderLoik::InfeasibilityTailSolve]: normInf delta_x_qp_: " 363 | << problem_.delta_x_qp_.template lpNorm() << std::endl; 364 | std::cerr << "[FirstOrderLoik::InfeasibilityTailSolve]: normInf delta_z_qp_: " 365 | << problem_.delta_z_qp_.template lpNorm() << std::endl; 366 | } 367 | 368 | }; // InfeasibilityTailSolve 369 | 370 | /// 371 | /// \brief Initialize the problem to be solved. 372 | /// 373 | /// \param[in] q current generalized configuration (DVec) 374 | /// \param[in] H_ref Cost weight for tracking reference (DMat) 375 | /// \param[in] v_ref reference spatial velocity (DVec) 376 | /// \param[in] active_task_constraint_ids vector of joint ids where equality constraints 377 | /// are present (std::vector) \param[in] Ais vector of equality 378 | /// constraint matrix (std::vector) \param[in] bis vector of 379 | /// equality constraint targets (std::vector) \param[in] lb joint 380 | /// velocity lower bounds (DVec) \param[in] ub joint velocity upper 381 | /// bounds (DVec) \param[out] this->ik_id_data_.z projected joint velocities onto the 382 | /// box constraint set 383 | /// 384 | void SolveInit( 385 | const DVec & q, 386 | const Mat6x6 & H_ref, 387 | const Motion & v_ref, 388 | const std::vector & active_task_constraint_ids, 389 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & Ais, 390 | const PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) & bis, 391 | const DVec & lb, 392 | const DVec & ub) 393 | { 394 | // wipe everything, warm-start is taken care of by 'ik_id_data_.Reset()' 395 | ResetSolver(); 396 | 397 | // TODO: perform initial fwd pass, to calculate forward kinematics quantities 398 | FwdPassInit(q); 399 | 400 | // update problem formulation 401 | problem_.UpdateQPADMMSolveInit( 402 | H_ref, v_ref, active_task_constraint_ids, Ais, bis, lb, ub, model_, ik_id_data_); 403 | 404 | }; // SolveInit 405 | 406 | /// 407 | /// \brief Solve the constrained differential IK problem, just the main loop 408 | /// 409 | void Solve() 410 | { 411 | // solver main loop 412 | for (int i = 1; i < this->max_iter_; i++) 413 | { 414 | 415 | this->iter_ = i; 416 | 417 | loik_solver_info_.iter_list_.push_back(this->iter_); 418 | 419 | ik_id_data_.UpdatePrev(); 420 | 421 | // fwd pass 1 422 | FwdPass1(); 423 | 424 | // bwd pass 425 | BwdPass(); 426 | 427 | // fwd pass 2 428 | FwdPass2(); 429 | 430 | // box projection 431 | BoxProj(); 432 | 433 | // dual update 434 | DualUpdate(); 435 | 436 | // update standard qp formulation using primal dual variables from current iter 437 | problem_.UpdateQPADMMSolveLoop(ik_id_data_); 438 | 439 | // compute residuals 440 | ComputeResiduals(); 441 | 442 | if (this->logging_) 443 | { 444 | 445 | // logging residuals TODO: should be disabled for speed 446 | loik_solver_info_.primal_residual_task_list_.push_back(primal_residual_task_); 447 | loik_solver_info_.primal_residual_slack_list_.push_back(primal_residual_slack_); 448 | loik_solver_info_.primal_residual_list_.push_back(this->primal_residual_); 449 | loik_solver_info_.dual_residual_nu_list_.push_back(dual_residual_nu_); 450 | loik_solver_info_.dual_residual_v_list_.push_back(dual_residual_v_); 451 | loik_solver_info_.dual_residual_list_.push_back(this->dual_residual_); 452 | 453 | loik_solver_info_.mu_list_.push_back(this->mu_); 454 | loik_solver_info_.mu_eq_list_.push_back(mu_eq_); 455 | loik_solver_info_.mu_ineq_list_.push_back(mu_ineq_); 456 | } 457 | 458 | // check for convergence or infeasibility 459 | CheckConvergence(); 460 | if (this->iter_ > 1) 461 | { 462 | CheckFeasibility(); 463 | } 464 | 465 | if (this->converged_) 466 | { 467 | break; // converged, break out of solver main loop 468 | } 469 | else if (this->primal_infeasible_) 470 | { 471 | if (this->verbose_) 472 | { 473 | std::cerr 474 | << "WARNING [FirstOrderLoik::Solve]: primal infeasibility detected at iteration: " 475 | << this->iter_ << std::endl; 476 | } 477 | // problem is primal infeasible, run infeasibility tail solve 478 | InfeasibilityTailSolve(); 479 | break; 480 | } 481 | else if (this->dual_infeasible_) 482 | { 483 | if (this->verbose_) 484 | { 485 | std::cerr 486 | << "WARNING [FirstOrderLoik::Solve]: dual infeasibility detected at iteration: " 487 | << this->iter_ << std::endl; 488 | } 489 | // problem is dual infeasibile, run infeasibility tail solve 490 | InfeasibilityTailSolve(); 491 | break; 492 | } 493 | 494 | // update ADMM penalty 495 | UpdateMu(); 496 | } 497 | }; // Solve 498 | 499 | /// 500 | /// \brief Stand alone Solve, solves the constrained differential IK problem. 501 | /// 502 | /// \param[in] q current generalized configuration (DVec) 503 | /// \param[in] H_ref Cost weight for tracking reference (DMat) 504 | /// \param[in] v_ref reference spatial velocity (DVec) 505 | /// \param[in] active_task_constraint_ids vector of joint ids where equality constraints 506 | /// are present (std::vector) \param[in] Ais vector of equality 507 | /// constraint matrix (std::vector) \param[in] bis vector of 508 | /// equality constraint targets (std::vector) \param[in] lb joint 509 | /// velocity lower bounds (DVec) \param[in] ub joint velocity upper 510 | /// bounds (DVec) \param[out] this->ik_id_data_.z projected joint velocities onto the 511 | /// box constraint set 512 | /// 513 | void Solve( 514 | const DVec & q, 515 | const Mat6x6 & H_ref, 516 | const Motion & v_ref, 517 | const std::vector & active_task_constraint_ids, 518 | const PINOCCHIO_ALIGNED_STD_VECTOR(Mat6x6) & Ais, 519 | const PINOCCHIO_ALIGNED_STD_VECTOR(Vec6) & bis, 520 | const DVec & lb, 521 | const DVec & ub) 522 | 523 | { 524 | // wipe everything, warm-start is taken care of by 'ik_id_data_.Reset()' 525 | ResetSolver(); 526 | 527 | // TODO: perform initial fwd pass, to calculate forward kinematics quantities 528 | FwdPassInit(q); 529 | 530 | // update problem formulation 531 | problem_.UpdateQPADMMSolveInit( 532 | H_ref, v_ref, active_task_constraint_ids, Ais, bis, lb, ub, model_, ik_id_data_); 533 | 534 | // solver main loop 535 | for (int i = 1; i < this->max_iter_; i++) 536 | { 537 | 538 | this->iter_ = i; 539 | 540 | if (this->verbose_) 541 | { 542 | std::cout << "===============" << std::endl; 543 | std::cout << "ADMM iter: " << this->iter_ << "||" << std::endl; 544 | std::cout << "===============" << std::endl; 545 | } 546 | 547 | loik_solver_info_.iter_list_.push_back(this->iter_); 548 | 549 | ik_id_data_.UpdatePrev(); 550 | 551 | // fwd pass 1 552 | FwdPass1(); 553 | 554 | // bwd pass 555 | BwdPass(); 556 | 557 | // fwd pass 2 558 | FwdPass2(); 559 | 560 | // box projection 561 | BoxProj(); 562 | 563 | // dual update 564 | DualUpdate(); 565 | 566 | // update standard qp formulation using primal dual variables from current iter 567 | problem_.UpdateQPADMMSolveLoop(ik_id_data_); 568 | 569 | // compute residuals 570 | ComputeResiduals(); 571 | 572 | if (this->logging_) 573 | { 574 | 575 | // logging residuals TODO: should be disabled for speed 576 | loik_solver_info_.primal_residual_task_list_.push_back(primal_residual_task_); 577 | loik_solver_info_.primal_residual_slack_list_.push_back(primal_residual_slack_); 578 | loik_solver_info_.primal_residual_list_.push_back(this->primal_residual_); 579 | loik_solver_info_.dual_residual_nu_list_.push_back(dual_residual_nu_); 580 | loik_solver_info_.dual_residual_v_list_.push_back(dual_residual_v_); 581 | loik_solver_info_.dual_residual_list_.push_back(this->dual_residual_); 582 | 583 | loik_solver_info_.mu_list_.push_back(this->mu_); 584 | loik_solver_info_.mu_eq_list_.push_back(mu_eq_); 585 | loik_solver_info_.mu_ineq_list_.push_back(mu_ineq_); 586 | } 587 | 588 | // check for convergence or infeasibility 589 | CheckConvergence(); 590 | if (this->iter_ > 1) 591 | { 592 | CheckFeasibility(); 593 | } 594 | 595 | if (this->converged_) 596 | { 597 | break; // converged, break out of solver main loop 598 | } 599 | else if (this->primal_infeasible_) 600 | { 601 | if (this->verbose_) 602 | { 603 | std::cerr 604 | << "WARNING [FirstOrderLoik::Solve]: primal infeasibility detected at iteration: " 605 | << this->iter_ << std::endl; 606 | } 607 | // problem is primal infeasible, run infeasibility tail solve 608 | InfeasibilityTailSolve(); 609 | break; 610 | } 611 | else if (this->dual_infeasible_) 612 | { 613 | if (this->verbose_) 614 | { 615 | std::cerr 616 | << "WARNING [FirstOrderLoik::Solve]: dual infeasibility detected at iteration: " 617 | << this->iter_ << std::endl; 618 | } 619 | // problem is dual infeasibile, run infeasibility tail solve 620 | InfeasibilityTailSolve(); 621 | break; 622 | } 623 | 624 | // update ADMM penalty 625 | UpdateMu(); 626 | } 627 | 628 | }; // Solve 629 | 630 | inline DVec get_primal_residual_vec() const 631 | { 632 | return primal_residual_vec_; 633 | }; 634 | inline DVec get_dual_residual_vec() const 635 | { 636 | return dual_residual_vec_; 637 | }; 638 | inline Scalar get_dual_residual_v() const 639 | { 640 | return dual_residual_v_; 641 | }; 642 | inline Scalar get_dual_residual_nu() const 643 | { 644 | return dual_residual_nu_; 645 | }; 646 | inline Scalar get_tol_tail_solve() const 647 | { 648 | return tol_tail_solve_; 649 | }; 650 | inline void set_tol_tail_solve(const Scalar tol) 651 | { 652 | tol_tail_solve_ = tol; 653 | }; 654 | 655 | /// test utilities 656 | inline Scalar get_delta_x_qp_inf_norm() const 657 | { 658 | return problem_.delta_x_qp_.template lpNorm(); 659 | }; 660 | inline Scalar get_delta_z_qp_inf_norm() const 661 | { 662 | return problem_.delta_z_qp_.template lpNorm(); 663 | }; 664 | inline DVec get_delta_y_qp() const 665 | { 666 | return problem_.delta_y_qp_; 667 | }; 668 | inline Scalar get_delta_y_qp_inf_norm() const 669 | { 670 | return problem_.delta_y_qp_.template lpNorm(); 671 | }; 672 | inline DVec get_A_qp_delta_y_qp() const 673 | { 674 | return problem_.A_qp_ * problem_.delta_y_qp_; 675 | }; 676 | inline Scalar get_A_qp_T_delta_y_qp_inf_norm() const 677 | { 678 | return (problem_.A_qp_.transpose() * problem_.delta_y_qp_).template lpNorm(); 679 | }; 680 | inline Scalar get_ub_qp_T_delta_y_qp_plus() const 681 | { 682 | return (problem_.ub_qp_.transpose() * problem_.delta_y_qp_plus_).value(); 683 | }; 684 | inline Scalar get_lb_qp_T_delta_y_qp_minus() const 685 | { 686 | return (problem_.lb_qp_.transpose() * problem_.delta_y_qp_minus_).value(); 687 | }; 688 | 689 | bool get_primal_infeasibility_cond_1() const 690 | { 691 | return (problem_.A_qp_.transpose() * problem_.delta_y_qp_).template lpNorm() 692 | <= this->tol_primal_inf_ * (problem_.delta_y_qp_).template lpNorm(); 693 | }; 694 | 695 | bool get_primal_infeasibility_cond_2() const 696 | { 697 | return (problem_.ub_qp_.transpose() * problem_.delta_y_qp_plus_ 698 | + problem_.lb_qp_.transpose() * problem_.delta_y_qp_minus_) 699 | .value() 700 | <= this->tol_primal_inf_ * (problem_.delta_y_qp_).template lpNorm(); 701 | }; 702 | 703 | protected: 704 | const Model & model_; 705 | IkIdData & ik_id_data_; 706 | 707 | ProblemFormulation problem_; 708 | 709 | // ADMM solver specific quantities 710 | int tail_solve_iter_; // tail solve iteration index 711 | Scalar primal_residual_kinematics_; // primal residual of just the forward kinematics equality 712 | // constraints 713 | Scalar primal_residual_task_; // primal residual of just the task equality constraints 714 | Scalar primal_residual_slack_; // primal residual of just the inequality induced slack equality 715 | // constraints 716 | DVec primal_residual_vec_; // utility vector for primal residual calculation 717 | 718 | Scalar dual_residual_prev_; 719 | Scalar delta_dual_residual_; 720 | Scalar dual_residual_v_; // dual residual of just the dual feasibility condition wrt v 721 | Scalar dual_residual_v_prev_; 722 | Scalar delta_dual_residual_v_; 723 | Scalar dual_residual_nu_; // dual residual of just the dual feasibility condition wrt nu 724 | Scalar dual_residual_nu_prev_; 725 | Scalar delta_dual_residual_nu_; 726 | DVec dual_residual_vec_; // utility vector for dual residual calculation 727 | 728 | Scalar mu_eq_; // ADMM penalty for equality constraints 729 | Scalar mu_ineq_; // ADMM penalty for inequality constraints 730 | 731 | // solver helper quantities 732 | int nj_; // number of joints in the model_ 733 | int nb_; // number of bodies in the model_, 'nb_ = nj_ - 1' 734 | int nv_; // dimension of nu_ (q_dot) 735 | IndexVec joint_full_range_; // index of full joint range, [0, njoints - 1] 736 | IndexVec joint_range_; // index of joint range excluding the world/universe [1, njoints - 1] 737 | 738 | // warm_start flag 739 | bool warm_start_; 740 | 741 | // tol for infeasibility tail solve 742 | Scalar tol_tail_solve_; 743 | 744 | // solver info logging struct 745 | LoikSolverInfo loik_solver_info_; 746 | }; 747 | 748 | } // namespace loik 749 | 750 | #include "loik/loik-loid.hxx" 751 | 752 | #if LOIK_ENABLE_TEMPLATE_INSTANTIATION 753 | #include "loik/loik-loid.txx" 754 | #endif // LOIK_ENABLE_TEMPLATE_INSTANTIATION 755 | --------------------------------------------------------------------------------