diff --git a/.clang-format-common.sh b/.clang-format-common.sh index e8039075..d642cb81 100644 --- a/.clang-format-common.sh +++ b/.clang-format-common.sh @@ -1,12 +1,12 @@ # This script is meant to be sourced from other scripts -# Check for clang-format, prefer 6.0 if available -if [[ -x "$(command -v clang-format-6.0)" ]]; then - clang_format=clang-format-6.0 +# Check for clang-format, prefer 10 if available +if [[ -x "$(command -v clang-format-10)" ]]; then + clang_format=clang-format-10 elif [[ -x "$(command -v clang-format)" ]]; then clang_format=clang-format else - echo "clang-format or clang-format-6.0 must be installed" + echo "clang-format or clang-format-10 must be installed" exit 1 fi diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 5869a33e..880edb3b 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -10,7 +10,7 @@ on: jobs: cmake-format: - runs-on: ubuntu-18.04 + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v2 - name: Install cmakelang @@ -21,15 +21,13 @@ jobs: ./.cmake-format-check.sh clang-format: needs: cmake-format - runs-on: ubuntu-18.04 + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v2 - - name: Install clang-format-6.0 + - name: Install clang-format-10 run: | - sudo rm -f /etc/apt/sources.list.d/dotnetdev.list /etc/apt/sources.list.d/microsoft-prod.list sudo apt-get -qq update - sudo apt-get -qq remove clang-6.0 libclang1-6.0 libclang-common-6.0-dev libllvm6.0 - sudo apt-get -qq install clang-format-6.0 clang-format + sudo apt-get -qq install clang-format-10 - name: Run clang-format-check run: | ./.clang-format-check.sh @@ -38,7 +36,7 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-16.04, ubuntu-18.04, macos-latest, windows-latest] + os: [ubuntu-20.04, macos-latest, windows-latest] build-type: [Debug, RelWithDebInfo, script] compiler: [gcc, clang] exclude: @@ -47,13 +45,10 @@ jobs: compiler: clang - os: windows-latest compiler: clang - - os: ubuntu-16.04 - build-type: script - - os: ubuntu-18.04 + - os: ubuntu-20.04 build-type: script - os: macos-latest build-type: script - runs-on: ${{ matrix.os }} steps: - uses: actions/checkout@v2 @@ -97,7 +92,7 @@ jobs: fi - name: Upload documentation # Only run on master branch and for one configuration - if: matrix.os == 'ubuntu-18.04' && matrix.build-type == 'RelWithDebInfo' && matrix.compiler == 'gcc' && github.ref == 'refs/heads/master' + if: matrix.os == 'ubuntu-20.04' && matrix.build-type == 'RelWithDebInfo' && matrix.compiler == 'gcc' && github.ref == 'refs/heads/master' uses: jrl-umi3218/github-actions/upload-documentation@master with: GH_USER: gergondet @@ -109,4 +104,4 @@ jobs: slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }} slack-channel: '#ci' slack-text: > - [mc_rtc] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }} + [state-observation] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }} diff --git a/.github/workflows/package.yml b/.github/workflows/package.yml index 7342243a..140bc423 100644 --- a/.github/workflows/package.yml +++ b/.github/workflows/package.yml @@ -18,226 +18,10 @@ on: tags: - v* jobs: - check-tag: - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - if: startsWith(github.ref, 'refs/tags/') - - name: Check version coherency - run: | - set -x - export VERSION=`echo ${{ github.ref }} | sed -e 's@refs/tags/v@@'` - echo "REJECTION=PROJECT_VERSION in CMakeLists.txt does not match tag" >> $GITHUB_ENV - grep -q "set(PROJECT_VERSION ${VERSION})" CMakeLists.txt - echo "REJECTION=Upstream version in debian/changelog does not match tag" >> $GITHUB_ENV - head -n 1 debian/changelog | grep -q "state-observation (${VERSION}" - echo "REJECTION=" >> $GITHUB_ENV - export TAG=`echo ${{ github.ref }} | sed -e 's@refs/tags/@@'` - echo "RELEASE_TAG=${TAG}" >> $GITHUB_ENV - if: startsWith(github.ref, 'refs/tags/') - - name: Delete tag - run: | - set -x - curl --header 'authorization: Bearer ${{ secrets.GITHUB_TOKEN }}' -X DELETE https://api.github.com/repos/${{ github.repository }}/git/${{ github.ref }} - if: failure() - - name: Notify tag deletion - uses: archive/github-actions-slack@master - with: - slack-bot-user-oauth-access-token: "${{ secrets.SLACK_BOT_TOKEN }}" - slack-channel: "#ci" - slack-text: | - Tag *${{ github.ref }}* in *${{ github.repository }}* was deleted: - ${{ env.REJECTION}} - if: failure() - - name: Create release - uses: jrl-umi3218/github-actions/create-release@master - with: - GITHUB_TOKEN: "${{ secrets.GITHUB_TOKEN }}" - tag: "${{ env.RELEASE_TAG }}" - if: startsWith(github.ref, 'refs/tags/') - build-packages: - needs: check-tag - strategy: - fail-fast: false - matrix: - dist: - - xenial - - bionic - - focal - arch: - - i386 - - amd64 - exclude: - - dist: focal - arch: i386 - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - - name: Choose extra mirror - run: | - # We upload in all conditions except when building on PR or branch other than master - export PACKAGE_UPLOAD=true - if ${{ startsWith(github.ref, 'refs/tags/') }} - then - export USE_HEAD=false - elif [ "${{ github.event.action }}" == "package-master" ] - then - export USE_HEAD=true - elif [ "${{ github.event.action }}" == "package-release" ] - then - export USE_HEAD=false - export REF=`git tag --sort=committerdate --list 'v[0-9]*'|tail -1` - git checkout $REF - git submodule sync && git submodule update - else - export REF=`echo ${{ github.ref }} | sed -e 's@refs/[a-z]*/@@'` - export USE_HEAD=true - if [ $REF != "master" ] - then - export PACKAGE_UPLOAD=false - fi - fi - if [ "${{ github.repository }}" != "jrl-umi3218/state-observation" ] - then - export PACKAGE_UPLOAD=false - fi - if $USE_HEAD - then - echo "CLOUDSMITH_REPO=mc-rtc/head" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-master" >> $GITHUB_ENV - else - echo "CLOUDSMITH_REPO=mc-rtc/stable" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-release" >> $GITHUB_ENV - fi - echo "PACKAGE_UPLOAD=${PACKAGE_UPLOAD}" >> $GITHUB_ENV - - name: Build package - uses: jrl-umi3218/github-actions/build-package-native@master - with: - dist: "${{ matrix.dist }}" - arch: "${{ matrix.arch }}" - cloudsmith-repo: "${{ env.CLOUDSMITH_REPO }}" - - uses: actions/upload-artifact@v1 - with: - name: packages-${{ matrix.dist }}-${{ matrix.arch }} - path: "/tmp/packages-${{ matrix.dist }}-${{ matrix.arch }}/" - if: env.PACKAGE_UPLOAD == 'true' - upload-packages: - needs: build-packages - strategy: - max-parallel: 1 - fail-fast: false - matrix: - dist: - - xenial - - bionic - - focal - arch: - - i386 - - amd64 - exclude: - - dist: focal - arch: i386 - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - - name: Choose extra mirror - run: | - # We upload in all conditions except when building on PR or branch other than master - export PACKAGE_UPLOAD=true - if ${{ startsWith(github.ref, 'refs/tags/') }} - then - export USE_HEAD=false - elif [ "${{ github.event.action }}" == "package-master" ] - then - export USE_HEAD=true - elif [ "${{ github.event.action }}" == "package-release" ] - then - export USE_HEAD=false - export REF=`git tag --sort=committerdate --list 'v[0-9]*'|tail -1` - git checkout $REF - git submodule sync && git submodule update - else - export REF=`echo ${{ github.ref }} | sed -e 's@refs/[a-z]*/@@'` - export USE_HEAD=true - if [ $REF != "master" ] - then - export PACKAGE_UPLOAD=false - fi - fi - if [ "${{ github.repository }}" != "jrl-umi3218/state-observation" ] - then - export PACKAGE_UPLOAD=false - fi - if $USE_HEAD - then - echo "CLOUDSMITH_REPO=mc-rtc/head" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-master" >> $GITHUB_ENV - else - echo "CLOUDSMITH_REPO=mc-rtc/stable" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-release" >> $GITHUB_ENV - fi - echo "PACKAGE_UPLOAD=${PACKAGE_UPLOAD}" >> $GITHUB_ENV - - name: Download packages - uses: actions/download-artifact@v1 - with: - name: packages-${{ matrix.dist }}-${{ matrix.arch }} - if: env.PACKAGE_UPLOAD == 'true' - - name: Upload - uses: jrl-umi3218/github-actions/upload-package@master - with: - dist: ubuntu/${{ matrix.dist }} - repo: "${{ env.CLOUDSMITH_REPO }}" - path: packages-${{ matrix.dist }}-${{ matrix.arch }} - CLOUDSMITH_API_KEY: "${{ secrets.CLOUDSMITH_API_KEY }}" - if: env.PACKAGE_UPLOAD == 'true' - mirror-sync-and-trigger: - needs: upload-packages - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - - name: Choose extra mirror - run: | - # We upload in all conditions except when building on PR or branch other than master - export PACKAGE_UPLOAD=true - if ${{ startsWith(github.ref, 'refs/tags/') }} - then - export USE_HEAD=false - elif [ "${{ github.event.action }}" == "package-master" ] - then - export USE_HEAD=true - elif [ "${{ github.event.action }}" == "package-release" ] - then - export USE_HEAD=false - export REF=`git tag --sort=committerdate --list 'v[0-9]*'|tail -1` - git checkout $REF - git submodule sync && git submodule update - else - export REF=`echo ${{ github.ref }} | sed -e 's@refs/[a-z]*/@@'` - export USE_HEAD=true - if [ $REF != "master" ] - then - export PACKAGE_UPLOAD=false - fi - fi - if $USE_HEAD - then - echo "CLOUDSMITH_REPO=mc-rtc/head" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-master" >> $GITHUB_ENV - else - echo "CLOUDSMITH_REPO=mc-rtc/stable" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-release" >> $GITHUB_ENV - fi - echo "PACKAGE_UPLOAD=${PACKAGE_UPLOAD}" >> $GITHUB_ENV - - name: Trigger dependents rebuild - run: 'curl -H "Accept: application/vnd.github.everest-preview+json" -H "Authorization: token ${{ secrets.GH_PAGES_TOKEN }}" --request POST --data "{\"event_type\": \"${PACKAGE_JOB}\"}" https://api.github.com/repos/jrl-umi3218/mc_rtc/dispatches - - ' - if: env.PACKAGE_UPLOAD == 'true' + package: + uses: jrl-umi3218/github-actions/.github/workflows/package-project.yml@master + with: + deps: '["jrl-umi3218/mc_rtc"]' + secrets: + CLOUDSMITH_API_KEY: ${{ secrets.CLOUDSMITH_API_KEY }} + GH_TOKEN: ${{ secrets.GH_PAGES_TOKEN }} diff --git a/.github/workflows/sources/package.yml b/.github/workflows/sources/package.yml deleted file mode 100644 index 89527905..00000000 --- a/.github/workflows/sources/package.yml +++ /dev/null @@ -1,219 +0,0 @@ -name: Package state-observation - -# On any branch/pull request it will: -# - Build packages for selected Debian/Ubuntu distros -# -# On master, it will additionally: -# - Build packages for selected Debian/Ubuntu distro -# - Upload the packages to https://cloudsmith.io/~mc-rtc/repos/head/packages/ -# -# On tagged versions it will: -# - Create a GitHub release draft -# - Attach the sources to the release -# - Build packages for selected Debian/Ubuntu distro -# - Upload the packages to https://cloudsmith.io/~mc-rtc/repos/stable/packages/ -# -# On package-master trigger, it will rebuild and upload the latest master package -# -# On package-release trigger, it will rebuild and upload the latest release package - -on: - repository_dispatch: - types: [package-master, package-release] - pull_request: - branches: - - '**' - push: - paths-ignore: - # Changes to those files don't mandate rebuilding a package - - "doc/**" - - "README.md" - - ".github/workflows/build.yml" - branches: - - '**' - tags: - - v* - -jobs: - # For a given tag vX.Y.Z, this checks: - # - set(PROJECT_VERSION X.Y.Z) in CMakeLists.txt - # - version X.Y.Z in debian/changelog - # If these checks fail, the tag is automatically deleted - # - # This job does not run on the master branch - check-tag: - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - if: startsWith(github.ref, 'refs/tags/') - - name: Check version coherency - run: | - set -x - export VERSION=`echo ${{ github.ref }} | sed -e 's@refs/tags/v@@'` - echo "REJECTION=PROJECT_VERSION in CMakeLists.txt does not match tag" >> $GITHUB_ENV - grep -q "set(PROJECT_VERSION ${VERSION})" CMakeLists.txt - echo "REJECTION=Upstream version in debian/changelog does not match tag" >> $GITHUB_ENV - head -n 1 debian/changelog | grep -q "state-observation (${VERSION}" - echo "REJECTION=" >> $GITHUB_ENV - export TAG=`echo ${{ github.ref }} | sed -e 's@refs/tags/@@'` - echo "RELEASE_TAG=${TAG}" >> $GITHUB_ENV - if: startsWith(github.ref, 'refs/tags/') - - name: Delete tag - run: | - set -x - curl --header 'authorization: Bearer ${{ secrets.GITHUB_TOKEN }}' -X DELETE https://api.github.com/repos/${{ github.repository }}/git/${{ github.ref }} - if: failure() - - name: Notify tag deletion - uses: archive/github-actions-slack@master - with: - slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }} - slack-channel: '#ci' - slack-text: > - Tag *${{ github.ref }}* in *${{ github.repository }}* was deleted: - ${{ env.REJECTION}} - if: failure() - - name: Create release - uses: jrl-umi3218/github-actions/create-release@master - with: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - tag: ${{ env.RELEASE_TAG }} - if: startsWith(github.ref, 'refs/tags/') - # This job build binary packages for Ubuntu - build-packages: - needs: check-tag - strategy: - fail-fast: false - matrix: &package-matrix - dist: [xenial, bionic, focal] - arch: [i386, amd64] - exclude: - - dist: focal - arch: i386 - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - - &choose-extra-mirror - name: Choose extra mirror - run: | - # We upload in all conditions except when building on PR or branch other than master - export PACKAGE_UPLOAD=true - if ${{ startsWith(github.ref, 'refs/tags/') }} - then - export USE_HEAD=false - elif [ "${{ github.event.action }}" == "package-master" ] - then - export USE_HEAD=true - elif [ "${{ github.event.action }}" == "package-release" ] - then - export USE_HEAD=false - export REF=`git tag --sort=committerdate --list 'v[0-9]*'|tail -1` - git checkout $REF - git submodule sync && git submodule update - else - export REF=`echo ${{ github.ref }} | sed -e 's@refs/[a-z]*/@@'` - export USE_HEAD=true - if [ $REF != "master" ] - then - export PACKAGE_UPLOAD=false - fi - fi - if [ "${{ github.repository }}" != "jrl-umi3218/state-observation" ] - then - export PACKAGE_UPLOAD=false - fi - if $USE_HEAD - then - echo "CLOUDSMITH_REPO=mc-rtc/head" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-master" >> $GITHUB_ENV - else - echo "CLOUDSMITH_REPO=mc-rtc/stable" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-release" >> $GITHUB_ENV - fi - echo "PACKAGE_UPLOAD=${PACKAGE_UPLOAD}" >> $GITHUB_ENV - - name: Build package - uses: jrl-umi3218/github-actions/build-package-native@master - with: - dist: ${{ matrix.dist }} - arch: ${{ matrix.arch }} - cloudsmith-repo: ${{ env.CLOUDSMITH_REPO }} - - uses: actions/upload-artifact@v1 - with: - name: packages-${{ matrix.dist }}-${{ matrix.arch }} - path: /tmp/packages-${{ matrix.dist }}-${{ matrix.arch }}/ - if: env.PACKAGE_UPLOAD == 'true' - # This job upload binary packages for Ubuntu - upload-packages: - needs: build-packages - strategy: - max-parallel: 1 - fail-fast: false - matrix: *package-matrix - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - - name: Set upload parameters - <<: *choose-extra-mirror - - name: Download packages - uses: actions/download-artifact@v1 - with: - name: packages-${{ matrix.dist }}-${{ matrix.arch }} - if: env.PACKAGE_UPLOAD == 'true' - - name: Upload - uses: jrl-umi3218/github-actions/upload-package@master - with: - dist: ubuntu/${{ matrix.dist }} - repo: ${{ env.CLOUDSMITH_REPO }} - path: packages-${{ matrix.dist }}-${{ matrix.arch }} - CLOUDSMITH_API_KEY: ${{ secrets.CLOUDSMITH_API_KEY }} - if: env.PACKAGE_UPLOAD == 'true' - mirror-sync-and-trigger: - needs: upload-packages - runs-on: ubuntu-20.04 - steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - - name: Choose extra mirror - run: | - # We upload in all conditions except when building on PR or branch other than master - export PACKAGE_UPLOAD=true - if ${{ startsWith(github.ref, 'refs/tags/') }} - then - export USE_HEAD=false - elif [ "${{ github.event.action }}" == "package-master" ] - then - export USE_HEAD=true - elif [ "${{ github.event.action }}" == "package-release" ] - then - export USE_HEAD=false - export REF=`git tag --sort=committerdate --list 'v[0-9]*'|tail -1` - git checkout $REF - git submodule sync && git submodule update - else - export REF=`echo ${{ github.ref }} | sed -e 's@refs/[a-z]*/@@'` - export USE_HEAD=true - if [ $REF != "master" ] - then - export PACKAGE_UPLOAD=false - fi - fi - if $USE_HEAD - then - echo "CLOUDSMITH_REPO=mc-rtc/head" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-master" >> $GITHUB_ENV - else - echo "CLOUDSMITH_REPO=mc-rtc/stable" >> $GITHUB_ENV - echo "PACKAGE_JOB=package-release" >> $GITHUB_ENV - fi - echo "PACKAGE_UPLOAD=${PACKAGE_UPLOAD}" >> $GITHUB_ENV - - name: Trigger dependents rebuild - run: 'curl -H "Accept: application/vnd.github.everest-preview+json" -H "Authorization: token ${{ secrets.GH_PAGES_TOKEN }}" --request POST --data "{\"event_type\": \"${PACKAGE_JOB}\"}" https://api.github.com/repos/jrl-umi3218/mc_rtc/dispatches - -' - if: env.PACKAGE_UPLOAD == 'true' diff --git a/.github/workflows/sources/setup.rb b/.github/workflows/sources/setup.rb deleted file mode 100755 index 2113ae7b..00000000 --- a/.github/workflows/sources/setup.rb +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env ruby - -require "yaml" -require "json" - -data = File.open(File.expand_path("package.yml", __dir__)).read().gsub(/^on:$/, "\"on\":"); -yaml = YAML.load(data) - -out = YAML.load(yaml.to_json).to_yaml(line_width: 1024).gsub(/^'on':$/, "on:"); -File.write(File.expand_path("../package.yml", __dir__), out) diff --git a/.gitignore b/.gitignore index a4853df4..fec290af 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,11 @@ build/ +build-dbg/ +build-debug/ +build-none/ +build-release/ +build-rls +build-relWithDebInfo/ .vs CMakeSettings.json +compile_commands.json +.vscode/tasks.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..2489d95c --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,16 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c11", + "cppStandard": "gnu++14", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..d1f44cfe --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,23 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "name": "(gdb) Attach", + "type": "cppdbg", + "request": "attach", + "program": "/home/arnaud/devel/bin/choreonoid", + "processId": "${command:pickProcess}", + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..9d354a84 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,63 @@ +{ + "files.associations": { + "*.tcc": "cpp", + "cmath": "cpp", + "complex": "cpp", + "core": "cpp", + "unordered_map": "cpp", + "iosfwd": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp", + "*.txx": "cpp" + }, + "window.zoomLevel": 0 +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 95ebd4a5..8258c345 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ endif() set(PROJECT_NAME state-observation) set(PROJECT_DESCRIPTION "General implementation of observers.") set(PROJECT_URL "https://github.com/jrl-umi3218/state-observation") -set(PROJECT_VERSION 1.4.1) +set(PROJECT_VERSION 1.5.0) set(CMAKE_CXX_STANDARD 11) set(CXX_DISABLE_WERROR TRUE) set(PROJECT_USE_CMAKE_EXPORT TRUE) @@ -73,7 +73,7 @@ install(FILES "${CMAKE_BINARY_DIR}/$/include/state-observation/config.h" DESTINATION ${INCLUDE_INSTALL_DESTINATION}) add_subdirectory(src) - +add_subdirectory(benchmarks) if(BUILD_TESTING) add_subdirectory(unit-testings) diff --git a/CMakeLists.txt.save b/CMakeLists.txt.save deleted file mode 100755 index 65ca03c4..00000000 --- a/CMakeLists.txt.save +++ /dev/null @@ -1,117 +0,0 @@ -cmake_minimum_required(VERSION 2.6) - -enable_language(CXX) -if ("${CMAKE_BUILD_TYPE}" STREQUAL "DEBUG") - set (CONFIG_EXT "d") #Extension corresponding to the configuration debug -endif ("${CMAKE_BUILD_TYPE}" STREQUAL "DEBUG") - - - -SET(PROJECT_NAME observation) -SET(LIB_NAME observation${CONFIG_EXT}) - - -IF(NOT "$ENV{CMAKE_INSTALL_PATH}" STREQUAL "") - SET(MY_OWN_INSTALL_PREFIX "$ENV{CMAKE_INSTALL_PATH}" CACHE PATH "Prefix prepended to install directories") - SET(CMAKE_INSTALL_PREFIX "${MY_OWN_INSTALL_PREFIX}" CACHE INTERNAL "Prefix prepended to install directories" FORCE) - message("CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}") -ENDIF(NOT "$ENV{CMAKE_INSTALL_PATH}" STREQUAL "") - -SET(CMAKE_VERBOSE_MAKEFILE ON) - -project(${PROJECT_NAME}) - - - - -IF(WIN32) - SET(LIBDIR_KW "/LIBPATH:") - SET(LIBINCL_KW "") - SET(LIB_EXT ".lib") -ENDIF(WIN32) - -IF(UNIX) - SET(LIBDIR_KW "-L") - SET(LIBINCL_KW "-l") - SET(LIB_EXT "") - SET(${PROJECT_NAME}_CXXFLAGS "-DUNIX" ) -ENDIF(UNIX) - -set(Boost_USE_STATIC_LIBS ON) - -#find_package(Boost COMPONENTS random REQUIRED) -find_package(Boost REQUIRED) - - -SET( EIGEN3_INCLUDE_DIR "$ENV{EIGEN3_INCLUDE_DIR}" ) -IF( NOT EIGEN3_INCLUDE_DIR ) - MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.") -ENDIF() - -#ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.0") - - -#Activate warning -if(MSVC) - # Force to always compile with W4 - if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]") - string(REGEX REPLACE "/W[0-4]" "/W4" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") - else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W4") - endif() -elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) - # Update if necessary - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-long-long -pedantic") -endif() - - -IF(${Boost_MINOR_VERSION} LESS 36) - SET(${PROJECT_NAME}_CXXFLAGS "-D WITH_BOOST_1_36 ${${PROJECT_NAME}_CXXFLAGS}") -ENDIF(${Boost_MINOR_VERSION} LESS 36) - -include_directories("${PROJECT_SOURCE_DIR}/include" "${Boost_INCLUDE_DIR}" "${EIGEN3_INCLUDE_DIR}") -# Optimisation flags when building with RELEASE -#---------------------------------------------- -IF(CMAKE_BUILD_TYPE MATCHES "RELEASE") - IF(CMAKE_COMPILER_IS_GNUCXX) - SET (${PROJECT_NAME}_CXXFLAGS "-O3 -funroll-loops -frerun-loop-opt -fschedule-insns2 -frerun-cse-after-loop -falign-functions -falign-labels -falign-loops -falign-jumps -fexpensive-optimizations ${${PROJECT_NAME}_CXXFLAGS}") - ENDIF(CMAKE_COMPILER_IS_GNUCXX) -ENDIF(CMAKE_BUILD_TYPE MATCHES "RELEASE") - -IF(CMAKE_BUILD_TYPE MATCHES "DEBUG") - IF(CMAKE_COMPILER_IS_GNUCXX) - SET (${PROJECT_NAME}_CXXFLAGS "-g ${${PROJECT_NAME}_CXXFLAGS}") - ENDIF(CMAKE_COMPILER_IS_GNUCXX) -ENDIF(CMAKE_BUILD_TYPE MATCHES "DEBUG") - -add_subdirectory(src) -add_subdirectory(unit-testings) - -SET_TARGET_PROPERTIES(${LIB_NAME} PROPERTIES LINKER_LANGUAGE CXX) - - -# ---------------------------------------------------------------- -# --- Documentation ---------------------------------------------- -# ---------------------------------------------------------------- - -option(GENERATE_DOC "Generate the documentation " OFF) -if (GENERATE_DOC) - include(FindDoxygen) - if(DOXYGEN_FOUND) - add_subdirectory(doc) - endif(DOXYGEN_FOUND) -endif (GENERATE_DOC) - -# Build a pkgconfig file -#SET(install_pkg_libdir "\${libdir}") -#SET(install_pkg_include_dir "\${includedir}") - -#define the linker language (useful for empty source folders) -#SET_TARGET_PROPERTIES(observation PROPERTIES LINKER_LANGUAGE CXX) - -#CONFIGURE_FILE(${${PROJECT_NAME}_SOURCE_DIR}/${PROJECT_NAME}.pc.cmake -# ${${PROJECT_NAME}_BINARY_DIR}/${PROJECT_NAME}.pc) -#INSTALL(FILES ${${PROJECT_NAME}_BINARY_DIR}/${PROJECT_NAME}.pc -# DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/pkgconfig -# PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE -#) diff --git a/TAGS b/TAGS deleted file mode 100644 index 1381319d..00000000 --- a/TAGS +++ /dev/null @@ -1,302 +0,0 @@ - -include/state-observer/extended-kalman-filter.hpp,626 -#define EXTENDEDKALMANFILTERHPP2,33 -namespace observation6,117 - template : public KalmanFilterBase<observation::KalmanFilterBase69,2185 - class DynamicsFunctorBaseobservation::DynamicsFunctorBase72,2267 - virtual typename ObserverBase<observation::DynamicsFunctorBase::ObserverBase75,2312 - virtual typename ObserverBase<observation::DynamicsFunctorBase::ObserverBase76,2447 - -include/state-observer/extended-kalman-filter.hxx,2178 -template ::setFunctor(setFunctor2,45 -template ::clearFunctor(clearFunctor10,228 -template ::setDirectInputOutputFeedthrough(setDirectInputOutputFeedthrough16,337 -template ::StateVector ExtendedKalmanFilter::prediction_(prediction_22,496 -template ::StateVector ExtendedKalmanFilter::getPrediction(getPrediction35,900 -template ::simulateSensor_(simulateSensor_46,1357 -template ::getAMatrixFD(getAMatrixFD71,2149 -template ::getCMatrixFD(getCMatrixFD97,2911 -template ::reset(reset121,3554 -template ::StateVector ExtendedKalmanFilter::prediction_(prediction_131,3704 -template ::StateVector ExtendedKalmanFilter::getPrediction(getPrediction144,4091 -template ::simulateSensor_(simulateSensor_153,4419 -template ::setFunctor(setFunctor162,4693 -template ::clearFunctor(clearFunctor168,4846 -template ::getAMatrixFD(getAMatrixFD177,5050 -template ::getCMatrixFD(getCMatrixFD200,5756 -template ::reset(reset223,6372 - -include/state-observer/kalman-filter-base.hpp,2013 -#define KALMANFILTERBASEHPP24,641 -namespace observation28,722 - template Amatrix;observation::Amatrix52,1393 - typedef Eigen::Matrix Cmatrix;observation::Cmatrix55,1478 - typedef Eigen::Matrix Qmatrix;observation::Qmatrix59,1597 - typedef Eigen::Matrix Rmatrix;observation::Rmatrix62,1712 - typedef Eigen::Matrix Pmatrix;observation::Pmatrix66,1839 - typedef Eigen::Matrix Kmatrix;observation::Kmatrix115,3052 - typedef DiscreteTimeMatrix Amat_;observation::Amat_128,3633 - typedef DiscreteTimeMatrix Cmat_;observation::Cmat_129,3675 - typedef DiscreteTimeMatrix Qmat_;observation::Qmat_132,3791 - typedef DiscreteTimeMatrix Rmat_;observation::Rmat_133,3839 - typedef DiscreteTimeMatrix Pmat_;observation::Pmat_137,3969 - */ template : public ZeroDelayObserver<observation::ZeroDelayObserver164,4689 - typedef Eigen::Matrix Amatrix;observation::Amatrix169,4807 - typedef Eigen::Matrix Cmatrix;observation::Cmatrix172,4891 - typedef Eigen::Matrix Qmatrix;observation::Qmatrix176,5009 - typedef Eigen::Matrix Rmatrix;observation::Rmatrix179,5124 - typedef Eigen::Matrix Pmatrix;observation::Pmatrix183,5250 - typedef Eigen::Matrix Kmatrix;observation::Kmatrix227,6445 - typedef DiscreteTimeMatrix Amat_;observation::Amat_241,7020 - typedef DiscreteTimeMatrix Cmat_;observation::Cmat_242,7063 - typedef DiscreteTimeMatrix Qmat_;observation::Qmat_245,7180 - typedef DiscreteTimeMatrix Rmat_;observation::Rmat_246,7228 - typedef DiscreteTimeMatrix Pmat_;observation::Pmat_250,7366 - -include/state-observer/kalman-filter-base.hxx,3001 -template ::setA(setA2,46 -template ::clearA(clearA8,203 -template ::setC(setC14,311 -template ::clearC(clearC20,469 -template ::setR(setR26,577 -template ::clearR(clearR32,736 -template ::setQ(setQ38,844 -template ::clearQ(clearQ44,1004 -template ::setStateCovariance(setStateCovariance50,1112 -template ::clearStateCovariance(clearStateCovariance56,1302 -template ::StateVector KalmanFilterBase::oneStepEstimation_(oneStepEstimation_62,1424 -template ::Pmatrix KalmanFilterBase::getStateCovariance(getStateCovariance121,2981 -template ::reset(reset128,3178 -template ::setA(setA142,3396 -template ::clearA(clearA148,3542 -template ::setC(setC154,3638 -template ::clearC(clearC160,3785 -template ::setR(setR166,3881 -template ::clearR(clearR172,4027 -template ::setQ(setQ178,4123 -template ::clearQ(clearQ184,4270 -template ::setStateCovariance(setStateCovariance190,4366 -template ::clearStateCovariance(clearStateCovariance196,4508 -template ::StateVector KalmanFilterBase::oneStepEstimation_(oneStepEstimation_203,4620 -template ::Pmatrix KalmanFilterBase::getStateCovariance(getStateCovariance242,5677 -template ::reset(reset249,5862 - -include/state-observer/kalman-filter.hpp,645 -#define KALMANFILTERHPP2,25 -namespace observation6,101 - template Bmatrix;observation::Bmatrix16,351 - typedef Eigen::Matrix Dmatrix;observation::Dmatrix17,398 - typedef DiscreteTimeMatrix Bmat_;observation::Bmat_31,839 - typedef DiscreteTimeMatrix Dmat_;observation::Dmat_32,882 - template : public KalmanFilterBase<observation::KalmanFilterBase39,995 - -include/state-observer/kalman-filter.hxx,1081 -template ::setB(setB2,46 -template ::clearB(clearB8,196 -template ::setD(setD14,300 -template ::clearD(clearD20,450 -template ::StateVector KalmanFilter::prediction_(prediction_27,556 -template ::simulateSensor_(simulateSensor_34,800 -template ::reset(reset56,1372 -template ::StateVector KalmanFilter::prediction_(prediction_65,1515 -template ::simulateSensor_(simulateSensor_72,1722 - -include/state-observer/observer-base.hpp,2246 -#define OBSERVERBASEHPP20,463 -namespace observation27,596 - class ObserverException:observation::ObserverException36,771 - ObserverException(observation::ObserverException::ObserverException39,837 - virtual const char* what(observation::ObserverException::what43,920 - const char* what_;observation::ObserverException::what_49,1026 - class InitializationException:observation::InitializationException60,1205 - InitializationException(observation::InitializationException::InitializationException63,1277 - class TimeException:observation::TimeException76,1572 - TimeException(observation::TimeException::TimeException79,1634 - template MatrixT;observation::DiscreteTimeMatrix::MatrixT105,2356 - typedef Eigen::Matrix MatrixTUnaligned;observation::DiscreteTimeMatrix::MatrixTUnaligned133,3198 - bool isSet_;observation::DiscreteTimeMatrix::isSet_135,3278 - unsigned k_;observation::DiscreteTimeMatrix::k_136,3294 - MatrixTUnaligned v_;observation::DiscreteTimeMatrix::v_137,3310 - template StateVector;observation::StateVector178,4366 - typedef Eigen::Matrix MeasureVector;observation::MeasureVector179,4417 - typedef Eigen::Matrix InputVector;observation::InputVector180,4470 - typedef DiscreteTimeMatrix State;observation::State212,5570 - typedef DiscreteTimeMatrix Measure;observation::Measure213,5612 - typedef DiscreteTimeMatrix Input;observation::Input214,5656 - -include/state-observer/observer-base.hxx,776 -template::DiscreteTimeMatrix(DiscreteTimeMatrix3,35 -template::DiscreteTimeMatrix(DiscreteTimeMatrix11,184 -template::set(set18,298 -template::reset(reset26,435 -template::MatrixT DiscreteTimeMatrix::operator()(operator()33,540 -template::getTime(getTime42,764 -template::isSet(isSet52,961 -template ::reset(reset58,1085 - -include/state-observer/zero-delay-observer.hpp,236 -#define ZERODELAYOBSERVER_H18,489 -namespace observation25,583 - template ::setStatesetState2,46 -template ::clearState(clearState19,394 -template ::setMeasurementsetMeasurement25,507 -template ::clearMeasurements(clearMeasurements46,1151 -template ::setInputsetInput52,1271 -template ::clearInputs(clearInputs73,1893 -template ::getEstimateState(getEstimateState80,2049 -template ::getCurrentTime(getCurrentTime100,2429 - -unit-testings/test-kalman-filter.cpp,907 -typedef observation::KalmanFilter<4,3,2> filter;11,184 -typedef observation::ExtendedKalmanFilter<4,3,1> ekf;12,233 -filter f;13,287 -ekf f2;14,297 -class KalmanFunctor:KalmanFunctor16,306 - KalmanFunctor(KalmanFunctor::KalmanFunctor20,370 - virtual ekf::StateVector stateDynamics(KalmanFunctor::stateDynamics28,538 - virtual ekf::MeasureVector measureDynamics(KalmanFunctor::measureDynamics36,776 - void setA(KalmanFunctor::setA43,968 - void setC(KalmanFunctor::setC48,1017 - void doNothing KalmanFunctor::doNothing53,1066 - ekf::StateVector s_;KalmanFunctor::s_56,1099 - ekf::StateVector t_;KalmanFunctor::t_57,1121 - ekf::MeasureVector m_;KalmanFunctor::m_58,1143 - ekf::MeasureVector n_;KalmanFunctor::n_59,1167 - ekf::Amatrix a_;KalmanFunctor::a_61,1192 - ekf::Cmatrix c_;KalmanFunctor::c_62,1210 -void testExtendedKalmanFilter(67,1234 -void testKalmanFilter(172,3434 -int main(272,5503 diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt new file mode 100755 index 00000000..7791d16d --- /dev/null +++ b/benchmarks/CMakeLists.txt @@ -0,0 +1,15 @@ +macro(ADD_BENCHMARK name) + add_executable(${name} ${name}.cpp) + + target_link_libraries(${name} PUBLIC state-observation benchmark ${ARGN}) + + add_test( + NAME ${name} + COMMAND $ + #CONFIGURATIONS $ + WORKING_DIRECTORY $) + # Adding a project configuration file (for MSVC only) + generate_msvc_dot_user_file(${name}) +endmacro() + +add_benchmark(bench-covMultSpeed) diff --git a/benchmarks/bench-covMultSpeed.cpp b/benchmarks/bench-covMultSpeed.cpp new file mode 100644 index 00000000..0da3ae49 --- /dev/null +++ b/benchmarks/bench-covMultSpeed.cpp @@ -0,0 +1,180 @@ +#include +#include +#include + +#include +#include +#include + +#include + +#include + +using namespace stateObservation::kine; + +static void Method1(benchmark::State & state) +{ + // Perform setup here + Eigen::MatrixXd P = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd Q = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd A = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + P = P * P.transpose(); // symmetric matrix + Q = Q * Q.transpose(); // symmetric matrix + Eigen::MatrixXd B1; + B1.resize(72, 72); + for(auto _ : state) + { + // This code gets timed + + // Method 1 + + B1.noalias() = Q + A * P * A.transpose(); + } + // std::cout << std::endl << "Method1: " << B1.sum() << std::endl; +} + +static void Method2(benchmark::State & state) +{ + // Perform setup here + Eigen::MatrixXd P = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd Q = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd A = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + P = P * P.transpose(); // symmetric matrix + Q = Q * Q.transpose(); // symmetric matrix + Eigen::MatrixXd B2; + B2.resize(72, 72); + for(auto _ : state) + { + // This code gets timed + + // Method 2 + + B2.triangularView() = A * P * A.transpose(); + Eigen::MatrixXd C(B2.selfadjointView()); + } +} + +static void Method3(benchmark::State & state) +{ + // Perform setup here + Eigen::MatrixXd P = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd Q = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd A = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + P = P * P.transpose(); // symmetric matrix + Q = Q * Q.transpose(); // symmetric matrix + Eigen::MatrixXd B3; + B3.resize(72, 72); + for(auto _ : state) + { + // This code gets timed + + // Method 3 + B3.triangularView() = A * P.selfadjointView() * A.transpose(); + Eigen::MatrixXd D(B3.selfadjointView()); + } +} + +static void Method3bis(benchmark::State & state) +{ + // Perform setup here + Eigen::MatrixXd P = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd Q = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd A = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + P = P * P.transpose(); // symmetric matrix + Q = Q * Q.transpose(); // symmetric matrix + Eigen::MatrixXd B3; + B3.resize(72, 72); + for(auto _ : state) + { + // This code gets timed + + // Method 3 + B3.triangularView() = Q; + B3.triangularView() += A * P.selfadjointView() * A.transpose(); + } +} + +static void Method5(benchmark::State & state) +{ + // Perform setup here + Eigen::MatrixXd P = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd Q = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd A = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + P = P * P.transpose(); // symmetric matrix + Q = Q * Q.transpose(); // symmetric matrix + Eigen::MatrixXd B5; + Eigen::MatrixXd B5t(72, 72); + B5.resize(72, 72); + for(auto _ : state) + { + // This code gets timed + + // Method 5 + + B5t.noalias() = A * P.selfadjointView(); + B5.triangularView() = B5t * A.transpose(); + B5.triangularView() += Q; + } +} + +static void Method7(benchmark::State & state) +{ + // Perform setup here + Eigen::MatrixXd P = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd Q = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + Eigen::MatrixXd A = + stateObservation::tools::ProbabilityLawSimulation::getUniformMatrix>() / 10; + P = P * P.transpose(); // symmetric matrix + Q = Q * Q.transpose(); // symmetric matrix + Eigen::MatrixXd B7; + B7.resize(72, 72); + Eigen::MatrixXd B7temp; + B7temp.resize(72, 72); + Eigen::LLT llt(72 * 72); + for(auto _ : state) + { + // This code gets timed + + // Method 7 + B7.triangularView() = Q; + llt.compute(P); + B7temp.noalias() = A * llt.matrixL(); + B7.selfadjointView().rankUpdate(B7temp); + } +} + +// Run the benchmark + +// BENCHMARK_MAIN(); + +int main(int argc, char ** argv) +{ + // Register the function as a benchmark + BENCHMARK(Method1); + BENCHMARK(Method2); + BENCHMARK(Method3); + BENCHMARK(Method3bis); + BENCHMARK(Method5); + BENCHMARK(Method7); + + ::benchmark::Initialize(&argc, argv); + if(::benchmark::ReportUnrecognizedArguments(argc, argv)) return 1; + ::benchmark::RunSpecifiedBenchmarks(); +} diff --git a/build b/build new file mode 120000 index 00000000..fc57682c --- /dev/null +++ b/build @@ -0,0 +1 @@ +/home/arnaud/devel/build-RelWithDebInfo/state-observation \ No newline at end of file diff --git a/compile_commands.json b/compile_commands.json new file mode 120000 index 00000000..50422fe5 --- /dev/null +++ b/compile_commands.json @@ -0,0 +1 @@ +/home/arnaud/devel/build-RelWithDebInfo/state-observation/compile_commands.json \ No newline at end of file diff --git a/debian/changelog b/debian/changelog index 61e37180..2c81f36a 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,9 @@ +state-observation (1.5.0-1debian1) unstable; urgency=medium + + * Add support for CoM offset and ZMP coefficient (#16) + + -- Pierre Gergondet Tue, 29 Nov 2022 14:06:39 +0900 + state-observation (1.4.1-1ubuntu1) unstable; urgency=medium * Simplify and correct Kinematics classes and functions diff --git a/include/state-observation/dynamical-system/dynamical-system-functor-base.hpp b/include/state-observation/dynamical-system/dynamical-system-functor-base.hpp index 211a4f2c..c54e3f44 100644 --- a/include/state-observation/dynamical-system/dynamical-system-functor-base.hpp +++ b/include/state-observation/dynamical-system/dynamical-system-functor-base.hpp @@ -35,7 +35,7 @@ class STATE_OBSERVATION_DLLAPI DynamicalSystemFunctorBase DynamicalSystemFunctorBase(); virtual ~DynamicalSystemFunctorBase(); - /// The function to oberload to describe the dynamics of the state + /// The function to overload to describe the dynamics of the state virtual Vector stateDynamics(const Vector & x, const Vector & u, TimeIndex k) = 0; /// The function to overload to describe the dynamics of the sensor (measurements) diff --git a/include/state-observation/dynamics-estimators/kinetics-observer.hpp b/include/state-observation/dynamics-estimators/kinetics-observer.hpp index 11168661..25a8e60d 100644 --- a/include/state-observation/dynamics-estimators/kinetics-observer.hpp +++ b/include/state-observation/dynamics-estimators/kinetics-observer.hpp @@ -12,7 +12,6 @@ #ifndef KINETICSOBSERVER_HPP #define KINETICSOBSERVER_HPP -#include #include #include @@ -32,16 +31,21 @@ namespace stateObservation /// @brief This observer estimated the kinematics and the external forces. /// @details The provided kinematics is the position, the orientation, the velocities and even the accelerations of a -/// local frame, called also observed frame in the global frame. This local frame can be any frame that is attached to -/// the robot with a known transformation. It could be attached to the CoM, to the base link of the robot or the control -/// frame that is supposed to be attached to the world frame but that actually is not. This estimation is based on the +/// frame 1 within another frame 2. The object Kinematics is the expression of these kinematics in the global frame 2, +/// while the LocalKinematics object is their expression in the local frame 1. Our observer estimates the local +/// kinematics of the centroid's frame within the world frame. The reason to choose the centroid's frame is that it +/// simplifies many expressions, for example the expressions of the accelerations. This estimation is based on the /// assumption of viscoelastic contacts and using three kinds of measurements: IMUs, Force/Torque measurements (contact /// and other ones) and any absolute position measurements. /// -class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunctorBase, protected StateVectorArithmetics +class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunctorBase, + protected kine::Kinematics::RecursiveAccelerationFunctorBase, + protected kine::LocalKinematics::RecursiveAccelerationFunctorBase, + protected StateVectorArithmetics { public: typedef kine::Kinematics Kinematics; + typedef kine::LocalKinematics LocalKinematics; typedef kine::Orientation Orientation; // //////////////////////////////////////////////////////////// @@ -97,6 +101,8 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @param b void setWithAccelerationEstimation(bool b = true); + bool getWithAccelerationEstimation() const; + /// @brief Set if the gyrometers bias is computed or not. /// This parameter is global for all the IMUs. /// @@ -108,6 +114,24 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @return sets void setMass(double); + const double & getMass() const; + + const IndexedMatrix3 & getInertiaMatrix() const; + + const IndexedMatrix3 & getInertiaMatrixDot() const; + + const IndexedVector3 & getAngularMomentum() const; + + const IndexedVector3 & getAngularMomentumDot() const; + + const IndexedVector3 & getCenterOfMass() const; + + const IndexedVector3 & getCenterOfMassDot() const; + + const IndexedVector3 & getCenterOfMassDotDot() const; + + const Vector6 getAdditionalWrench() const; + /// @} // /////////////////////////////////////////////////////////// @@ -118,26 +142,25 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @{ - /// @brief Set the measurements of an IMU and give the Kinematic of the IMU + /// @brief Set the measurements of an IMU and give the Kinematic of the IMU in the user frame. /// /// @details The overload that does not have the covariance matrices as an /// inputs uses default ones. /// /// The IMU is located in a sensor frame. We suppose we know the kinematics of - /// this sensor frame in the local frame (for example the base frame or the - /// control frame). + /// this sensor frame in the centroid's frame /// /// @return the number of the IMU (useful in case there are several ones) /// @param accelero measured value /// @param gyrometer measured gyro value - /// @param localKine sets the kinematics of the IMU expressed in the observed local - /// frame. The best is to provide the position, the orientation, + /// @param centroidImuKinematics sets the kinematics of the IMU in the user's + /// frame, expressed in the user's frame. The best is to provide the position, the orientation, /// the angular and linear velocities and the linear acceleration /// Nevertheless if velocities or accelerations are not available they will be /// automatically computed through finite differences /// @param num the number of the IMU (useful in case there are several ones). /// If not set it will be generated automatically. - int setIMU(const Vector3 & accelero, const Vector3 & gyrometer, const Kinematics & localKine, int num = -1); + int setIMU(const Vector3 & accelero, const Vector3 & gyrometer, const Kinematics & userImuKinematics, int num = -1); /// @brief @copybrief setIMU(const Vector3&,const Vector3&,const Kinematics &,int) /// Provides also the associated covariance matrices @@ -150,7 +173,7 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct const Vector3 & gyrometer, const Matrix3 & acceleroCov, const Matrix3 & gyroCov, - const Kinematics & localKine, + const Kinematics & userImuKinematics, int num = -1); /// @brief set the default covariance matrix for IMU. @@ -191,11 +214,9 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @brief Set a new contact with the environment /// - /// @param pose is the initial guess on the position of the contact. Only position and orientation are enough. If the - /// contact is compliant, you need to set the "rest" pose of the contact (i.e. the pose that gives zero reaction - /// force) - /// @param contactWrench is the initial wrench on the contact expressed in the local frame of the contact frame - /// (e.g. the force-sensor measurement) + /// @param pose is the initial guess on the position of the contact in the WORLD frame. Only position and orientation + /// are enough. If the contact is compliant, you need to set the "rest" pose of the contact (i.e. the pose that gives + /// zero reaction force) /// @param initialCovarianceMatrix is the covariance matrix expressing the uncertainty in the pose of the initial /// guess in the 6x6 upper left corner ( if no good initial guess is available give a rough position with a high /// initial covariance matrix, if the position is certain, set it to zero.) and the initial wrench in the 6x6 lower @@ -215,7 +236,6 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// Matrix3::Constant(-1) (default) to use the default one /// @return int the id number of the contact just added (returns contactNumber if it is positive) int addContact(const Kinematics & pose, - const Vector6 & contactWrench, const Matrix12 & initialCovarianceMatrix, const Matrix12 & processCovarianceMatrix, int contactNumber = -1, @@ -226,7 +246,8 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @brief Set a new contact with the environment (use default covariance matrices) /// - /// @param pose is the initial guess on the position of the contact. Only position and orientation are enough + /// @param pose is the initial guess on the position of the contact in the WORLD frame. Only position and orientation + /// are enough /// @param contactNumber the number id of the contact to add. If no predefined id, use -1 (default) in order to set /// the number automatically /// @param linearStiffness the linear stiffness of the contact viscoelastic model, if unknown, set to @@ -239,7 +260,6 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// Matrix3:::Constant(-1) (default) to use the default one /// @return int the id number of the contact just added (returns contactNumber if it is positive) int addContact(const Kinematics & pose, - const Vector6 & contactWrench = Vector6::Zero(), int contactNumber = -1, const Matrix3 & linearStiffness = Matrix3::Constant(-1), const Matrix3 & linearDamping = Matrix3::Constant(-1), @@ -254,10 +274,15 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @brief remove all the contacts void clearContacts(); + /// @brief Get the Number Of Contacts + /// + /// @return Index The number of contacts + Index getNumberOfContacts() const; + /// @brief Get the Current Number Of Contacts /// /// @return Index The current number of contacts - Index getNumberOfContacts() const; + Index getNumberOfSetContacts() const; /// @brief Get the List Of Contact ids /// @@ -277,7 +302,7 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @{ /// @brief Update the contact when it is NOT equipped with wrench sensor - /// @param localKine the new kinematics of the contact expressed in the local observed frame + /// @param localKine the new kinematics of the contact expressed in the centroid's frame frame /// the best is to provide the position, the orientation, the angular and the linear velocities. /// Otherwise they will be computed automatically /// @param contactNumber The number id of the contact @@ -306,6 +331,27 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @param wrenchSensorCovMat the new default covariance matrix void setContactWrenchSensorDefaultCovarianceMatrix(const Matrix6 & wrenchSensorCovMat); + void setInitWorldCentroidStateVector(const Vector & initStateVector); + + void setAllCovariances(const Matrix3 & statePositionInitCovariance, + const Matrix3 & stateOriInitCovariance, + const Matrix3 & stateLinVelInitCovariance, + const Matrix3 & stateAngVelInitCovariance, + const Matrix3 & gyroBiasInitCovariance, + const Matrix6 & unmodeledWrenchInitCovariance, + const Matrix12 & contactInitCovariance, + const Matrix3 & statePositionProcessCovariance, + const Matrix3 & stateOriProcessCovariance, + const Matrix3 & stateLinVelProcessCovariance, + const Matrix3 & stateAngVelProcessCovariance, + const Matrix3 & gyroBiasProcessCovariance, + const Matrix6 & unmodeledWrenchProcessCovariance, + const Matrix12 & contactProcessCovariance, + const Matrix3 & positionSensorCovariance, + const Matrix3 & orientationSensorCoVariance, + const Matrix3 & acceleroSensorCovariance, + const Matrix3 & gyroSensorCovariance, + const Matrix6 & contactSensorCovariance); /// @} // ///////////////////////////////////////////// @@ -336,42 +382,42 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @brief Set the 3x3 inertia matrix and its derivative expressed in the local frame /// - /// @param I Inertia matrix + /// @param I Set the inertia matrix at the CoM /// @param I_dot Derivative of inertia matrix - void setInertiaMatrix(const Matrix3 & I, const Matrix3 & I_dot); + void setCoMInertiaMatrix(const Matrix3 & I, const Matrix3 & I_dot); /// @brief Set the 3x3 inertia matrix expressed in the local frame /// @details The derivative will be computed using finite differences /// /// @param I Inertia matrix /// @param I_dot Derivative of inertia matrix - void setInertiaMatrix(const Matrix3 & I); + void setCoMInertiaMatrix(const Matrix3 & I); /// @brief Set the inertia matrix and its derivative as a Vector6 expressed in the local frame /// /// @param I Inertia matrix as a vector containing the diagonal and the three non /// diagonal values concatenated /// @param I_dot Derivative of inertia matrix expressed in the same way - void setInertiaMatrix(const Vector6 & I, const Vector6 & I_dot); + void setCoMInertiaMatrix(const Vector6 & I, const Vector6 & I_dot); /// @brief Set the inertia matrix as a Vector6 expressed in the local frame /// @details The derivative will be computed using finite differences /// /// @param I Inertia matrix as a vector containing the diagonal and the three non /// diagonal values concatenated - void setInertiaMatrix(const Vector6 & I); + void setCoMInertiaMatrix(const Vector6 & I); - /// @brief Set the Angular Momentum and its derviative expressed in the local frame + /// @brief Set the Angular Momentum around the CoM and its derviative expressed in the local frame /// /// @param sigma The angular momentum /// @param sigma_dot The angular momentum derivative - void setAngularMomentum(const Vector3 & sigma, const Vector3 & sigma_dot); + void setCoMAngularMomentum(const Vector3 & sigma, const Vector3 & sigma_dot); - /// @brief Set the Angular Momentum expressed in the local frame + /// @brief Set the Angular Momentum around the CoM expressed in the local frame /// @details The derivative will be computed using finite differences /// /// @param sigma The angular momentum - void setAngularMomentum(const Vector3 & sigma); + void setCoMAngularMomentum(const Vector3 & sigma); /// @brief Set any Additional resultant known wrench (e.g. measured external forces and moments but no contact ones) /// expressed in the local estimated frame. @@ -388,6 +434,10 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// ///////////////////////////////////////////////////////// /// @{ + /// @brief Updates the measurements. + /// @details Updates the measurement sensors and the associated vectors and covariance matrices + void updateMeasurements(); + /// @brief Runs the estimation. /// @details This is the function that allows to /// 1- compute the estimation @@ -396,20 +446,34 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @return const Vector& The state vector const Vector & update(); + /// @brief Returns the predicted Kinematics object of the centroid in the world frame at the time of the measurement + /// predictions + + /// @brief Converts a given wrench from the user to the centroid frame + /// @details Performs the conversion of a wrench {force, torque} from the user frame to the centroid frame. + /// + void convertWrenchFromUserToCentroid(const Vector3 & forceUserFrame, + const Vector3 & momentUserFrame, + Vector3 & forceCentroidFrame, + Vector3 & momentCentroidFrame); + /// @brief Get the Kinematics of the observed local frame - /// @details the kinemactics are the main output of this observer. It includes the linear and angular position and + /// @details the kinematics are the main output of this observer. It includes the linear and angular position and /// velocity but not the accelerations by default. To get the acceleration call estimateAccelerations(). This /// method does NOT update the estimation, for this use update(). /// /// @return Kinematics - Kinematics getKinematics() const; + + LocalKinematics getLocalCentroidKinematics() const; + + Kinematics getGlobalCentroidKinematics() const; /// @brief gets the Kinematics that include the linear and angular accelerations. /// @details This method computes the estimated accelerations from the observed state of the robot. It means this /// acceleration is filtered by the model /// /// @return Kinematics - Kinematics estimateAccelerations(); + LocalKinematics estimateAccelerations(); /// @brief Get the global-frame kinematics of a local-frame defined kinematics /// @details The kinematics are linear and angular positions, velocities and optionally accalerations. This method @@ -418,7 +482,11 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// /// @param localKinematics /// @return Kinematics - Kinematics getKinematicsOf(const Kinematics & localKinematics) const; + LocalKinematics getLocalKinematicsOf(const LocalKinematics & localKinematics) const; + /* + Kinematics getGlobalKinematicsOf(const LocalKinematics & localKinematics) const; + */ + Kinematics getGlobalKinematicsOf(const Kinematics & kin) const; /// get the contact force provided by the estimator /// which is different from a contact sensor measurement @@ -457,10 +525,26 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @brief Set the State Kinematics /// @details Sets a value for the kinematics part of the state /// - /// @param kine is the new kinematics of the state + /// @param localKine are the new local kinematics of the state + /// @param resetContactWrenches set if the contact wrenches should be reset + /// @param resetCovariance set if the covariance of the state should be reset + void setWorldCentroidStateKinematics(const LocalKinematics & localKine, + bool resetContactWrenches = true, + bool resetCovariance = true); + + /// @{ + /// @brief Set the State Kinematics + /// @details Sets a value for the kinematics part of the state + /// + /// @param localKine are the new kinematics of the state /// @param resetContactWrenches set if the contact wrenches should be reset /// @param resetCovariance set if the covariance of the state should be reset - void setStateKinematics(const Kinematics & kine, bool resetContactWrenches = true, bool resetCovariance = true); + void setWorldCentroidStateKinematics(const Kinematics & kine, bool resetCovariance = true); + + void setStateContact(const int & index, + Kinematics worldContactRestKine, + const Vector6 & wrench, + bool resetCovariance = true); // TODO // void setVelocityGuess(const Kinematics) @@ -592,6 +676,12 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @return Index Index getStateSize() const; + /// @{ + /// @brief Get the State Vector Tangent Size. + /// + /// @return Index + Index getStateTangentSize() const; + /// @brief Get the Measurement vector Size. /// /// @return Index @@ -626,13 +716,13 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// This is for advanced use but may be used to check how many states have been estimated up to now /// /// @return TimeIndex - TimeIndex getStateVectorTimeIndex() const; + const TimeIndex getStateVectorTimeIndex() const; /// @brief Set a value of the state x_k provided from another source /// @details can be used for initialization of the estimator /// /// @param newvalue The new value for the state vector - /// @param resetCovariance set if the state covariaance should be reset + /// @param resetCovariance set if the state covariance should be reset void setStateVector(const Vector & newvalue, bool resetCovariance = true); /// @brief Get the Measurement Vector @@ -867,7 +957,9 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct { virtual ~IMU() {} IMU() : Sensor(sizeIMUSignal) {} - Kinematics kinematics; + int num; + Kinematics userImuKinematics; // the kinematics of the IMU in the user's frame + LocalKinematics centroidImuKinematics; // the kinematics of the IMU in the IMU's frame Vector6 acceleroGyro; Matrix3 covMatrixAccelero; Matrix3 covMatrixGyro; @@ -881,11 +973,25 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct struct Contact : public Sensor { - Contact() : Sensor(sizeWrench), isSet(false), withRealSensor(false), stateIndex(-1), stateIndexTangent(-1) {} + Contact() : Sensor(sizeWrench), isSet(false), withRealSensor(false), stateIndex(-1), stateIndexTangent(-1) + { + worldRestPose.angVel = worldRestPose.linVel = Vector3::Zero(); + } virtual ~Contact() {} - Kinematics absPose; - Vector6 wrench; + struct Temp + { + Kinematics worldContactPose; // the pose of the contact in the world frane obtained by forward kinematics from the + // centroid's frame. This is a temporary variable used for convenience. It must be + // called with care as it is called in several functions in the code. + }; + int num; + Temp temp; + + Kinematics worldRestPose; // the rest pose of the contact in the world frame + Kinematics worldRefPose_DEBUG; // the input rest pose of the contact in the world frame + + Vector6 wrenchMeasurement; /// Describes the measured wrench (forces + torques) at the contact in the sensor's frame CheckedMatrix6 sensorCovMatrix; Matrix3 linearStiffness; @@ -897,9 +1003,9 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct bool withRealSensor; int stateIndex; int stateIndexTangent; - - Kinematics localKine; /// describes the kinematics of the contact point in the local frame - static const Kinematics::Flags::Byte localKineFlags = /// flags for the components of the kinematics + Kinematics userContactKine; /// Describes the kinematics of the contact point in the centroid's frame. + Kinematics centroidContactKine; /// Describes the kinematics of the contact point in the centroid's frame. + static const Kinematics::Flags::Byte contactKineFlags = /// flags for the components of the kinematics Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel | Kinematics::Flags::angVel; @@ -921,24 +1027,99 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct protected: ///////////// DYNAMICAL SYSTEM IMPLEMENTATION + /// @brief Applies the state-transition model to the given state vector using the given input to predict the future + /// state. + /// @param x The current state vector + /// @param u The current input vector + /// @param k The current time index + /// @return Vector& virtual Vector stateDynamics(const Vector & x, const Vector & u, TimeIndex k); + /// @brief Applies the measurement model to the given state vector using the given input to predict the sensor + /// measurements. + /// @param x The current state vector + /// @param u The current input vector + /// @param k The current time index + /// @return Vector& virtual Vector measureDynamics(const Vector & x, const Vector & u, TimeIndex k); - void addUnmodeledAndContactWrench_(const Vector & stateVector, Vector3 & force, Vector3 & torque); - - void computeAccelerations_(Kinematics & stateKine, - const Vector3 & totalForceLocal, - const Vector3 & totalMomentLocal, - Vector3 & linAcc, - Vector3 & angAcc); - - /// the kinematics is not const to allow more optimized non const operators to work - void computeContactForces_(VectorContactIterator i, - Kinematics & stateKine, - Kinematics & contactPose, - Vector3 & Force, - Vector3 torque); + /// @brief Adds the unmodeled and contact wrenches from the state to the given wrench. + /// @param centroidStateVector The current state vector + /// @param force The force we want to add the forces to. Must be expressed in the centroid frame. + /// @param torque The torque we want to add the torques to. Must be expressed in the centroid frame. + void addUnmodeledAndContactWrench_(const Vector & centroidStateVector, Vector3 & force, Vector3 & torque); + + /// @brief Adds the unmodeled wrench from the state to the given wrench. + /// @param centroidStateVector The current state vector + /// @param force The force we want to add the unmodeled force to. Must be expressed in the centroid frame. + /// @param torque The torque we want to add the unmodeled torque to. Must be expressed in the centroid frame. + void addUnmodeledWrench_(const Vector & centroidStateVector, Vector3 & force, Vector3 & torque); + + /// @brief adds the contribution of a contact wrench at the centroid to the total wrench + /// + /// @param centroidContactKine The Kinematics of the current contact at the centroid + /// @param centroidContactForce The contact force at the centroid to add to the total force + /// @param centroidContactTorque The contact torque at the centroid to add to the total torque + /// @param totalCentroidForce The total force exerced at the centroid, that will be completed with the contact's force + /// @param totalCentroidTorque The total torque exerced at the centroid, that will be completed with the contact's + /// torque + void addContactWrench_(const Kinematics & centroidContactKine, + const Vector3 & centroidContactForce, + const Vector3 & centroidContactTorque, + Vector3 & totalCentroidForce, + Vector3 & totalCentroidTorque); + + /// @brief Computes the local accelerations of the centroid frame in the world frame and adds them to its local + /// kinematics. + /// + /// @param localStateKine The local kinematics of the centroid frame in the world frame that still don't contain the + /// accelerations. + /// @param centroidContactForce The contact force at the centroid to add to the total force + /// @param totalForceLocal Total force exerted on the centroid. + /// @param totalMomentLocal Total torque exerted on the centroid. + /// @param linAcc The empty vector of the linear acceleration we want to compute. + /// @param angAcc The empty vector of the angular acceleration we want to compute. + + void computeLocalAccelerations_(LocalKinematics & localStateKine, + const Vector3 & totalForceLocal, + const Vector3 & totalMomentLocal, + Vector3 & linAcc, + Vector3 & angAcc); + + /// @brief Computes the accelerations of the centroid in the world frame after a small increment of the kinematics. + /// @details Uses the predicted kinematics in the visco-elastic model to obtain an estimate of the contact + /// forces.These forces replace the contact forces of the previously know state to compute the accelerations. Used to + /// compute the acceleration of the centroid in the world frame over the steps of the Runge-Kutta intergation. + /// @param predictedWorldCentroidKinematics Newly predicted kinematics of the centroid in the world frame. + virtual void computeRecursiveGlobalAccelerations_(Kinematics & predictedWorldCentroidKinematics); + + /// @brief @copybrief computeLocalAccelerations_(LocalKinematics & localStateKine, const Vector3 & totalForceLocal, + /// const Vector3 & totalMomentLocal, Vector3 & linAcc, Vector3 & angAcc). Version adapted to local kinematics. + /// @details @copydetails computeLocalAccelerations_(LocalKinematics & localStateKine, const Vector3 & + /// totalForceLocal, const Vector3 & totalMomentLocal, Vector3 & linAcc, Vector3 & angAcc) + virtual void computeRecursiveLocalAccelerations_(LocalKinematics & predictedWorldCentroidKinematics); + + /// @brief Computes the force exerted at a contact using the visco-elastic model on the given state vector. + /// @param i Contact to estimate + /// @param worldCentroidStateKinematics State vector used in the visco-elastic model + /// @param worldRestContactPose Rest pose of the contact + /// @param contactForce Empty vector of the contact force to estimate + /// @param contactTorque Empty vector of the contact force to estimate + void computeContactForce_(VectorContactIterator i, + LocalKinematics & worldCentroidStateKinematics, + Kinematics & worldRestContactPose, + Vector3 & contactForce, + Vector3 & contactTorque); + + /// @brief @copybrief computeContactForce_(VectorContactIterator i, LocalKinematics & worldCentroidStateKinematics, + /// Kinematics & worldRestContactPose, Vector3 & contactForce, Vector3 & contactTorque). Compute the resulting wrench + /// for all currently set contacts. + /// @param worldCentroidStateKinematics State vector used in the visco-elastic model + /// @param contactForce Empty vector of the contact force to estimate + /// @param contactTorque Empty vector of the contact force to estimate + void computeContactForces_(LocalKinematics & worldCentroidStateKinematics, + Vector3 & contactForce, + Vector3 & contactTorque); /// Sets a noise which disturbs the state dynamics virtual void setProcessNoise(NoiseBase *); @@ -959,6 +1140,31 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct virtual Index getInputSize() const; public: + /// @{ + const Vector6 getWorldContactWrench(const int & numContact) const; + + const Vector6 getCentroidContactWrench(const int & numContact) const; + + const Kinematics getCentroidContactInputPose(const int & numContact) const; + + const Kinematics getWorldContactInputRefPose(const int & numContact) const; + + const Kinematics getWorldContactPose(const int & numContact) const; + + const Kinematics & getContactStateRestKinematics(const int & numContact) const; + + const Kinematics getUserContactInputPose(const int & numContact) const; + + /// @brief Get the measurement index of the required IMU : allows to access its corresponding measurements in the + /// measurement vector for example + /// + /// @return const int & + const int getIMUMeasIndexByNum(const int & num) const; + + const int getContactMeasIndexByNum(const int & num) const; + + const bool getContactIsSetByNum(const int & num) const; + /////////////////////////////////////////////////////////////// /// @name State vector representation arithmetics and derivation (advanced use) /////////////////////////////////////////////////////////////// @@ -1009,13 +1215,55 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// @param dx the timestep virtual void setFiniteDifferenceStep(const Vector & dx); + /// @brief Define if we use the Runge-Kutta approximation method or not + /// + /// @param b true means we use finite differences + virtual void useRungeKutta(bool b = true); + + virtual Matrix computeAMatrix(); + + virtual Matrix computeCMatrix(); + + /// @brief computes the local acceleration from a the state vector + void computeLocalAccelerations(const Vector & x, Vector & acceleration); + + /// @brief Comparison between the Jacobians of the linear and angular accelerations with respect to the state, + /// obtained with finite differences and analyticially. + friend int testAccelerationsJacobians(KineticsObserver & ko, + int errcode, + double relativeErrorThreshold, + double threshold); // declared out of namespace state-observation + + /// @brief Comparison between the analytical Jacobian matrix A and the one obtained by finite differences. Used to + /// test the analytical method. + /// @param threshold Threshold on the relative error between both Jacobians (in percentage) + friend int testAnalyticalAJacobianVsFD(KineticsObserver & ko, + int errcode, + double relativeErrorThreshold, + double threshold); // declared out of namespace state-observation + + friend int testAnalyticalCJacobianVsFD(KineticsObserver & ko, + int errcode, + double relativeErrorThreshold, + double threshold); // declared out of namespace state-observation + + /// @brief Comparison between the Jacobians of orientation integration with respect to an increment vector delta, + /// obtained with finite differences and analyticially. + friend int testOrientationsJacobians(KineticsObserver & ko, + int errcode, + double relativeErrorThreshold, + double threshold); // declared out of namespace state-observation /// @} protected: - Vector stateNaNCorrection_(); + void stateNaNCorrection_(); + + /// @brief update of the state kinematics worldCentroidStateKinematics_ and of the contacts pose with the newly + /// estimated state + void updateLocalKineAndContacts_(); - /// updates stateKine_ from the stateVector - void updateKine_(); + /// updates the global kinematics of the centroid from the local ones, that can be more interpretable + void updateGlobalKine_(); protected: unsigned maxContacts_; @@ -1030,15 +1278,24 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct Index measurementSize_; Index measurementTangentSize_; - Kinematics stateKinematics_; + Vector worldCentroidStateVector_; + Vector worldCentroidStateVectorDx_; + Vector oldWorldCentroidStateVector_; - Vector stateVector_; - Vector stateVectorDx_; - Vector oldStateVector_; + LocalKinematics worldCentroidStateKinematics_; + Kinematics worldCentroidKinematics_; Vector3 additionalForce_; Vector3 additionalTorque_; + Vector3 initTotalCentroidForce_; // Initial total force used in the Runge-Kutta integration, coming from the current + // state's variables + Vector3 initTotalCentroidTorque_; + + Vector3 initialVEContactForces_; // Initial contact forces used in the Runge-Kutta integration, coming from the + // visco-elastic model used on the state's kinematics. + Vector3 initialVEContactTorques_; + Vector measurementVector_; Matrix measurementCovMatrix_; @@ -1048,12 +1305,14 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct bool withUnmodeledWrench_; bool withAccelerationEstimation_; + bool withRungeKutta_; // defines whether to use the Runge-Kutta approximation method or not + IndexedVector3 com_, comd_, comdd_; IndexedVector3 sigma_, sigmad_; IndexedMatrix3 I_, Id_; - TimeIndex k_est_; - TimeIndex k_data_; + TimeIndex k_est_; // time index of the last estimation + TimeIndex k_data_; // time index of the current measurements double mass_; @@ -1070,8 +1329,21 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// calls the reset of the iteration void startNewIteration_(); - virtual Matrix computeAMatrix_(); - virtual Matrix computeCMatrix_(); + /// @brief Converts a LocalKinematics object from the user's frame to the centroid's frame, which is used for most of + /// the computations + /// @param userKine the LocalKinematics object expressed in the user's frame. It is likely to correspond to the IMU's + /// LocalKinematics, defined by the user in its frame. + /// @param centroidKine the LocalKinematics object corresponding to the converted LocalKinematics in the centroid's + /// frame. + + inline void convertUserToCentroidFrame_(const Kinematics & userKine, Kinematics & centroidKine, TimeIndex k_data); + + /// @brief Converts a Kinematics object from the user's frame to the centroid's frame, which is used for most of the + /// computations + /// @param userKine the Kinematics object expressed in the user's frame. It is likely to correspond to the contact's + /// Kinematics, defined by the user in its frame. + + inline Kinematics convertUserToCentroidFrame_(const Kinematics & userKine, TimeIndex k_data); /// Getters for the indexes of the state Vector using private types inline unsigned contactIndex(VectorContactConstIterator i) const; @@ -1091,22 +1363,27 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct inline unsigned contactTorqueIndexTangent(VectorContactConstIterator i) const; inline unsigned contactWrenchIndexTangent(VectorContactConstIterator i) const; -public: - ///////////SIZE OF VECTORS - +public: ///////////SIZE OF VECTORS static const unsigned sizeAcceleroSignal = 3; static const unsigned sizeGyroSignal = 3; static const unsigned sizeIMUSignal = sizeAcceleroSignal + sizeGyroSignal; static const unsigned sizePos = 3; + static const unsigned sizePosTangent = 3; static const unsigned sizeOri = 4; static const unsigned sizeOriTangent = 3; static const unsigned sizeLinVel = sizePos; + static const unsigned sizeLinVelTangent = sizeLinVel; + static const unsigned sizeLinAccTangent = sizeLinVelTangent; static const unsigned sizeAngVel = sizeOriTangent; + static const unsigned sizeAngVelTangent = sizeAngVel; static const unsigned sizeGyroBias = sizeGyroSignal; + static const unsigned sizeGyroBiasTangent = sizeGyroBias; static const unsigned sizeForce = 3; + static const unsigned sizeForceTangent = sizeForce; static const unsigned sizeTorque = 3; + static const unsigned sizeTorqueTangent = sizeTorque; static const unsigned sizeWrench = sizeForce + sizeTorque; @@ -1131,9 +1408,11 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct static const Kinematics::Flags::Byte flagsPoseKine = Kinematics::Flags::position | Kinematics::Flags::orientation; + static const Kinematics::Flags::Byte flagsPosKine = Kinematics::Flags::position; + static const Kinematics::Flags::Byte flagsIMUKine = Kinematics::Flags::position | Kinematics::Flags::orientation | Kinematics::Flags::linVel | Kinematics::Flags::angVel - | Kinematics::Flags::linAcc; + | Kinematics::Flags::linAcc | Kinematics::Flags::angAcc; ////////////DEFAULT VALUES ////// static const double defaultMass; @@ -1171,6 +1450,8 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct static const double angularDampingDefault; //////////// + + bool nanDetected_ = false; EIGEN_MAKE_ALIGNED_OPERATOR_NEW protected: @@ -1215,10 +1496,10 @@ class STATE_OBSERVATION_DLLAPI KineticsObserver : protected DynamicalSystemFunct /// a structure to optimize computations struct Opt { - Opt() : kine(kine1), ori(kine.orientation), ori1(kine1.orientation), ori2(kine2.orientation) {} + Opt() : locKine(locKine1), ori(locKine.orientation), ori1(locKine1.orientation), ori2(locKine2.orientation) {} - Kinematics kine1, kine2; - Kinematics & kine; + LocalKinematics locKine1, locKine2; + LocalKinematics & locKine; Orientation & ori; Orientation & ori1; Orientation & ori2; diff --git a/include/state-observation/dynamics-estimators/kinetics-observer.hxx b/include/state-observation/dynamics-estimators/kinetics-observer.hxx index aa316c80..618614b2 100644 --- a/include/state-observation/dynamics-estimators/kinetics-observer.hxx +++ b/include/state-observation/dynamics-estimators/kinetics-observer.hxx @@ -8,25 +8,25 @@ inline unsigned KineticsObserver::posIndex() const } inline unsigned KineticsObserver::oriIndex() const { - return posIndex()+sizePos; + return posIndex() + sizePos; } inline unsigned KineticsObserver::linVelIndex() const { - return oriIndex()+sizeOri; + return oriIndex() + sizeOri; } inline unsigned KineticsObserver::angVelIndex() const { - return linVelIndex()+sizeLinVel; + return linVelIndex() + sizeLinVel; } -inline unsigned KineticsObserver::gyroBiasIndex( unsigned numberOfIMU) const +inline unsigned KineticsObserver::gyroBiasIndex(unsigned numberOfIMU) const { - BOOST_ASSERT( numberOfIMU < maxImuNumber_ && \ - "The requested IMU number is higher than the maximum"); - return angVelIndex() + sizeAngVel+ sizeGyroBias * numberOfIMU; + BOOST_ASSERT((numberOfIMU < maxImuNumber_ || numberOfIMU == 0) + && "The requested IMU number is higher than the maximum"); + return angVelIndex() + sizeAngVel + sizeGyroBias * numberOfIMU; } inline unsigned KineticsObserver::unmodeledWrenchIndex() const { - return gyroBiasIndex(0)+ sizeGyroBias*maxImuNumber_; + return gyroBiasIndex(0) + sizeGyroBias * maxImuNumber_; } inline unsigned KineticsObserver::unmodeledForceIndex() const { @@ -42,10 +42,8 @@ inline unsigned KineticsObserver::contactsIndex() const } inline unsigned KineticsObserver::contactIndex(unsigned contactNbr) const { - BOOST_ASSERT(contactNbr < maxContacts_ && \ - "The requested contact number is higher than the maximum"); - BOOST_ASSERT(contacts_[contactNbr].isSet && \ - "The requested contact is not set yet, please add it before"); + BOOST_ASSERT(contactNbr < maxContacts_ && "The requested contact number is higher than the maximum"); + BOOST_ASSERT(contacts_[contactNbr].isSet && "The requested contact is not set yet, please add it before"); return unsigned(contacts_[contactNbr].stateIndex); } inline unsigned KineticsObserver::contactKineIndex(unsigned contactNbr) const @@ -58,11 +56,11 @@ inline unsigned KineticsObserver::contactPosIndex(unsigned contactNbr) const } inline unsigned KineticsObserver::contactOriIndex(unsigned contactNbr) const { - return contactPosIndex(contactNbr)+sizePos; + return contactPosIndex(contactNbr) + sizePos; } inline unsigned KineticsObserver::contactWrenchIndex(unsigned contactNbr) const { - return contactKineIndex(contactNbr)+sizeContactKine; + return contactKineIndex(contactNbr) + sizeContactKine; } inline unsigned KineticsObserver::contactForceIndex(unsigned contactNbr) const { @@ -70,13 +68,12 @@ inline unsigned KineticsObserver::contactForceIndex(unsigned contactNbr) const } inline unsigned KineticsObserver::contactTorqueIndex(unsigned contactNbr) const { - return contactForceIndex(contactNbr)+sizeForce; + return contactForceIndex(contactNbr) + sizeForce; } inline unsigned KineticsObserver::contactIndex(VectorContactConstIterator i) const { - BOOST_ASSERT(i->stateIndex > 0 && \ - "The requested contact is not set yet. The iteratot may be wrong"); + BOOST_ASSERT(i->stateIndex > 0 && "The requested contact is not set yet. The iteratot may be wrong"); return unsigned(i->stateIndex); } @@ -90,11 +87,11 @@ inline unsigned KineticsObserver::contactPosIndex(VectorContactConstIterator i) } inline unsigned KineticsObserver::contactOriIndex(VectorContactConstIterator i) const { - return contactPosIndex(i)+sizePos; + return contactPosIndex(i) + sizePos; } inline unsigned KineticsObserver::contactWrenchIndex(VectorContactConstIterator i) const { - return contactKineIndex(i)+sizeContactKine; + return contactKineIndex(i) + sizeContactKine; } inline unsigned KineticsObserver::contactForceIndex(VectorContactConstIterator i) const { @@ -102,10 +99,9 @@ inline unsigned KineticsObserver::contactForceIndex(VectorContactConstIterator i } inline unsigned KineticsObserver::contactTorqueIndex(VectorContactConstIterator i) const { - return contactForceIndex(i)+sizeForce; + return contactForceIndex(i) + sizeForce; } - inline unsigned KineticsObserver::kineIndexTangent() const { return 0; @@ -116,25 +112,24 @@ inline unsigned KineticsObserver::posIndexTangent() const } inline unsigned KineticsObserver::oriIndexTangent() const { - return posIndexTangent()+sizePos; + return posIndexTangent() + sizePos; } inline unsigned KineticsObserver::linVelIndexTangent() const { - return oriIndexTangent()+sizeOriTangent; + return oriIndexTangent() + sizeOriTangent; } inline unsigned KineticsObserver::angVelIndexTangent() const { - return linVelIndexTangent()+sizeLinVel; + return linVelIndexTangent() + sizeLinVel; } -inline unsigned KineticsObserver::gyroBiasIndexTangent( unsigned numberOfIMU) const +inline unsigned KineticsObserver::gyroBiasIndexTangent(unsigned numberOfIMU) const { - BOOST_ASSERT( numberOfIMU < maxImuNumber_ && \ - "The requested IMU number is higher than the maximum"); - return angVelIndexTangent() + sizeAngVel+ sizeGyroBias * numberOfIMU; + BOOST_ASSERT(numberOfIMU <= maxImuNumber_ && "The requested IMU number is higher than the maximum"); + return angVelIndexTangent() + sizeAngVel + sizeGyroBias * numberOfIMU; } inline unsigned KineticsObserver::unmodeledWrenchIndexTangent() const { - return gyroBiasIndexTangent(0)+sizeGyroBias*maxImuNumber_; + return gyroBiasIndexTangent(0) + sizeGyroBias * maxImuNumber_; } inline unsigned KineticsObserver::unmodeledForceIndexTangent() const { @@ -142,18 +137,16 @@ inline unsigned KineticsObserver::unmodeledForceIndexTangent() const } inline unsigned KineticsObserver::contactsIndexTangent() const { - return unmodeledWrenchIndexTangent()+sizeWrench; + return unmodeledWrenchIndexTangent() + sizeWrench; } inline unsigned KineticsObserver::unmodeledTorqueIndexTangent() const { - return unmodeledForceIndexTangent()+sizeForce; + return unmodeledForceIndexTangent() + sizeForce; } inline unsigned KineticsObserver::contactIndexTangent(unsigned contactNbr) const { - BOOST_ASSERT(contactNbr < maxContacts_ && \ - "The requested contact number is higher than the maximum"); - BOOST_ASSERT(contacts_[contactNbr].isSet && \ - "The requested contact is not set yet, please add it before"); + BOOST_ASSERT(contactNbr < maxContacts_ && "The requested contact number is higher than the maximum"); + BOOST_ASSERT(contacts_[contactNbr].isSet && "The requested contact is not set yet, please add it before"); return contacts_[contactNbr].stateIndexTangent; } inline unsigned KineticsObserver::contactKineIndexTangent(unsigned contactNbr) const @@ -166,11 +159,11 @@ inline unsigned KineticsObserver::contactPosIndexTangent(unsigned contactNbr) co } inline unsigned KineticsObserver::contactOriIndexTangent(unsigned contactNbr) const { - return contactPosIndexTangent(contactNbr)+sizePos; + return contactPosIndexTangent(contactNbr) + sizePos; } inline unsigned KineticsObserver::contactWrenchIndexTangent(unsigned contactNbr) const { - return contactKineIndexTangent(contactNbr)+sizeContactKineTangent; + return contactKineIndexTangent(contactNbr) + sizeContactKineTangent; } inline unsigned KineticsObserver::contactForceIndexTangent(unsigned contactNbr) const { @@ -178,13 +171,12 @@ inline unsigned KineticsObserver::contactForceIndexTangent(unsigned contactNbr) } inline unsigned KineticsObserver::contactTorqueIndexTangent(unsigned contactNbr) const { - return contactForceIndexTangent(contactNbr)+sizeForce; + return contactForceIndexTangent(contactNbr) + sizeForce; } inline unsigned KineticsObserver::contactIndexTangent(VectorContactConstIterator i) const { - BOOST_ASSERT(i->stateIndexTangent > 0 && \ - "The requested contact is not set yet. The iteratot may be wrong"); + BOOST_ASSERT(i->stateIndexTangent > 0 && "The requested contact is not set yet. The iteratot may be wrong"); return i->stateIndexTangent; } inline unsigned KineticsObserver::contactKineIndexTangent(VectorContactConstIterator i) const @@ -197,11 +189,11 @@ inline unsigned KineticsObserver::contactPosIndexTangent(VectorContactConstItera } inline unsigned KineticsObserver::contactOriIndexTangent(VectorContactConstIterator i) const { - return contactPosIndexTangent(i)+sizePos; + return contactPosIndexTangent(i) + sizePos; } inline unsigned KineticsObserver::contactWrenchIndexTangent(VectorContactConstIterator i) const { - return contactKineIndexTangent(i)+sizeContactKineTangent; + return contactKineIndexTangent(i) + sizeContactKineTangent; } inline unsigned KineticsObserver::contactForceIndexTangent(VectorContactConstIterator i) const { @@ -209,19 +201,19 @@ inline unsigned KineticsObserver::contactForceIndexTangent(VectorContactConstIte } inline unsigned KineticsObserver::contactTorqueIndexTangent(VectorContactConstIterator i) const { - return contactForceIndexTangent(i)+sizeForce; + return contactForceIndexTangent(i) + sizeForce; } -inline Vector KineticsObserver::stateSum(const Vector& stateVector, const Vector & tangentVector) +inline Vector KineticsObserver::stateSum(const Vector & stateVector, const Vector & tangentVector) { - Vector sum; - stateSum(stateVector, tangentVector, sum); - return sum; + Vector sum; + stateSum(stateVector, tangentVector, sum); + return sum; } -inline Vector KineticsObserver::stateDifference(const Vector& stateVector1, const Vector& stateVector2) +inline Vector KineticsObserver::stateDifference(const Vector & stateVector1, const Vector & stateVector2) { - Vector diff; - stateDifference(stateVector1, stateVector2, diff); - return diff; + Vector diff; + stateDifference(stateVector1, stateVector2, diff); + return diff; } diff --git a/include/state-observation/observer/kalman-filter-base.hpp b/include/state-observation/observer/kalman-filter-base.hpp index a4b7bab3..3c4d4a08 100644 --- a/include/state-observation/observer/kalman-filter-base.hpp +++ b/include/state-observation/observer/kalman-filter-base.hpp @@ -295,7 +295,7 @@ class STATE_OBSERVATION_DLLAPI KalmanFilterBase : public ZeroDelayObserver, prot /// for the estimation call getEstimateState method /// it is only an execution of the state synamics with the current state /// estimation and the current input value - inline StateVector updateStatePrediction(); + inline const StateVector & updateStatePrediction(); /// update the predicted state, enables to precompute the predicted measurementŔ /// triggers also Vector updateStatePrediction() @@ -309,6 +309,8 @@ class STATE_OBSERVATION_DLLAPI KalmanFilterBase : public ZeroDelayObserver, prot /// get the last predicted measurement MeasureVector getLastPredictedMeasurement() const; + MeasureVector getLastMeasurement() const; + /// get the last Kalman gain matrix Matrix getLastGain() const; @@ -316,6 +318,8 @@ class STATE_OBSERVATION_DLLAPI KalmanFilterBase : public ZeroDelayObserver, prot /// (used for the case of multiplicative Kalman filter) void setStateArithmetics(StateVectorArithmetics * arith); + virtual void resetPrediction(); + protected: /// the size of tangent space of the state space Index nt_; @@ -373,6 +377,7 @@ class STATE_OBSERVATION_DLLAPI KalmanFilterBase : public ZeroDelayObserver, prot LLTPMatrix inoMeasCovLLT; Matrix kGain; Matrix t; + Matrix mKc; } oc_; StateVectorArithmetics * arithm_; @@ -381,7 +386,7 @@ class STATE_OBSERVATION_DLLAPI KalmanFilterBase : public ZeroDelayObserver, prot EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -/*inline*/ Vector KalmanFilterBase::updateStatePrediction() +/*inline*/ const Vector & KalmanFilterBase::updateStatePrediction() { prediction_(this->x_.getTime() + 1); return xbar_(); diff --git a/include/state-observation/observer/tilt-estimator.hpp b/include/state-observation/observer/tilt-estimator.hpp index d2318a5d..f64c741a 100644 --- a/include/state-observation/observer/tilt-estimator.hpp +++ b/include/state-observation/observer/tilt-estimator.hpp @@ -38,6 +38,8 @@ class STATE_OBSERVATION_DLLAPI TiltEstimator : public ZeroDelayObserver /// \li gamma : parameter related to the orthogonality TiltEstimator(double alpha, double beta, double gamma); + void initEstimator(Vector3 x1 = Vector3::Zero(), Vector3 x2_ = Vector3::Zero(), Vector3 x2 = Vector3::Zero()); + /// set the gain of x1_hat variable void setAlpha(const double alpha) { @@ -98,11 +100,17 @@ class STATE_OBSERVATION_DLLAPI TiltEstimator : public ZeroDelayObserver return R_S_C_; } + Vector3 getVirtualLocalVelocityMeasurement() + { + return x1_; + } + /// sets teh linear velocity of the IMU sensor in the control frame void setSensorLinearVelocityInC(const Vector3 & v) { v_S_C_ = v; } + Vector3 getSensorLinearVelocityInC() { return v_S_C_; @@ -129,6 +137,8 @@ class STATE_OBSERVATION_DLLAPI TiltEstimator : public ZeroDelayObserver return v_C_; } + void setExplicitX1(const Vector3 & x1); + /// prevent c++ overloaded virtual function warning #if defined(__clang__) # pragma clang diagnostic push @@ -182,6 +192,8 @@ class STATE_OBSERVATION_DLLAPI TiltEstimator : public ZeroDelayObserver Vector3 x2_hat_; Vector3 dx1_hat; + bool withExplicitX1_; + /// The tilt estimator loop StateVector oneStepEstimation_(); }; diff --git a/include/state-observation/tools/definitions.hpp b/include/state-observation/tools/definitions.hpp index cba7e92a..4962b8d5 100644 --- a/include/state-observation/tools/definitions.hpp +++ b/include/state-observation/tools/definitions.hpp @@ -341,17 +341,37 @@ class DebugItem T b_; }; -/// this is simply a structure allowing for automatically verifying that -/// the item has been initialized or not. The chckitm_reset() function allows to -/// set it back to "not initialized" state. -/// -lazy means that the "set" value is true all the time if NDEBUG is defined -/// -alwaysCheck means that the check is always performed and throws exception -/// if it fails. Otherwise, the check is performed only for debug. -/// warning, this has no effect if lazy is set to true -/// - assertion means that an assertion will be introduced for the check. -/// - eigenAlignedNew should be set to true if any alignment is required for the -/// new operator (see eigen documentation) -template +template +struct EmptyChecker +{ + static inline bool check(const T &) + { + return true; + } + static inline bool checkAssert(const T &) + { + return true; + } + static constexpr char errorMessage[] = ""; + static constexpr std::exception * exception = 0x0; +}; + +/// @brief this is a structure allowing for automatically verifying that the item has been initialized or not. The +/// chckitm_reset() function allows to set it back to "not initialized" state. +/// @tparam T is the contained type +/// @tparam lazy means that the "set" value is true all the time if NDEBUG is defined +/// @tparam alwaysCheck means that the check is always performed and throws exception if it fails. Otherwise, the check +/// is performed only for debug warning, this has no effect if lazy is set to true +/// @tparam assertion means that an assertion will be introduced for the check. +/// @tparam eigenAlignedNew should be set to true if any alignment is required for the new operator (see eigen +/// documentation) +/// @tparam additionalChecker defines a check function that is called in addition to the initialization check +template> class CheckedItem { public: @@ -366,11 +386,14 @@ class CheckedItem inline operator T() const; inline operator const T &() const; - inline T chckitm_getValue() const; + inline const T & chckitm_getValue() const; inline T & operator()(); inline const T & operator()() const; + inline T & getRefUnchecked(); + inline const T & getRefUnchecked() const; + inline bool isSet() const; inline void reset(); @@ -406,11 +429,41 @@ class CheckedItem EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(eigenAlignedNew) }; -typedef CheckedItem CheckedMatrix3; -typedef CheckedItem CheckedMatrix6; -typedef CheckedItem CheckedMatrix12; -typedef CheckedItem CheckedVector3; -typedef CheckedItem CheckedVector6; +template +struct checkNaN +{ + static inline bool check(const MatrixType & m) + { + if(doTest) + { + return !m.hasNaN(); // returns false if m has a NaN : test failed + } + else + { + return true; + } + } + static inline bool checkAssert(const MatrixType & m) + { + if(doTest) + { + BOOST_ASSERT(!m.hasNaN() && "Matrix contains a NaN."); + return !m.hasNaN(); // returns false if m has a NaN : test failed + } + else + { + return true; + } + } + static constexpr char errorMessage[] = "Matrix contains a NaN."; + static constexpr std::exception * exception = 0x0; +}; + +typedef CheckedItem> CheckedMatrix3; +typedef CheckedItem> CheckedMatrix6; +typedef CheckedItem> CheckedMatrix12; +typedef CheckedItem> CheckedVector3; +typedef CheckedItem> CheckedVector6; typedef CheckedItem CheckedQuaternion; /** @@ -421,7 +474,7 @@ typedef CheckedItem CheckedQuaternion; * * */ -template +template class IndexedMatrixT : protected DebugItem { typedef DebugItem IsSet; diff --git a/include/state-observation/tools/definitions.hxx b/include/state-observation/tools/definitions.hxx index 6933f0eb..8934f2e5 100644 --- a/include/state-observation/tools/definitions.hxx +++ b/include/state-observation/tools/definitions.hxx @@ -1,13 +1,14 @@ -template -inline CheckedItem::CheckedItem(bool initialize) +template +inline CheckedItem::CheckedItem(bool initialize) { if (initialize) isSet_=false; } -template -inline CheckedItem::CheckedItem(const CheckedItem & c): - v_(c.v_) +template +inline CheckedItem::CheckedItem( + const CheckedItem & c) +: v_(c.v_) { if (do_check_) { @@ -25,24 +26,22 @@ inline CheckedItem::CheckedItem( } } - - -template -inline CheckedItem::CheckedItem(const T& v): - isSet_(true), v_(v) +template +inline CheckedItem::CheckedItem(const T & v) +: isSet_(true), v_(v) { } -template -inline T& CheckedItem::operator=(const T& v) +template +inline T & CheckedItem::operator=(const T & v) { isSet_.set(true); return v_=v; } -template -inline CheckedItem & - CheckedItem::operator=(const CheckedItem & c) +template +inline CheckedItem & + CheckedItem::operator=(const CheckedItem & c) { v_=c.v_; @@ -64,45 +63,59 @@ inline CheckedItem & return *this; } -template -inline CheckedItem::operator T() const +template +inline CheckedItem::operator T() const { return (*this)(); } -template -inline CheckedItem::operator const T&() const +template +inline CheckedItem::operator const T &() const { return (*this)(); } - -template -inline T CheckedItem::chckitm_getValue() const +template +inline const T & CheckedItem::chckitm_getValue() + const { return (*this)(); } -template -inline const T& CheckedItem::operator()() const +template +inline const T & CheckedItem::operator()() const { chckitm_check_(); return v_; } -template -inline T& CheckedItem::operator()() +template +inline T & CheckedItem::operator()() { chckitm_check_(); return v_; } -template -inline bool CheckedItem::chckitm_check_() const +template +inline const T & CheckedItem::getRefUnchecked() + const +{ + return v_; +} + +template +inline T & CheckedItem::getRefUnchecked() +{ + return v_; +} + +template +inline bool CheckedItem::chckitm_check_() const { if (assertion) { BOOST_ASSERT_MSG(isSet(),assertMsg_.get()); + additionalChecker::checkAssert(v_); } if (alwaysCheck || isDebug) @@ -111,77 +124,75 @@ inline bool CheckedItem::chckitm { throw (*(exceptionPtr_.get())); } + if(!additionalChecker::check(v_)) + { + throw(*additionalChecker::exception); + } } return (isSet()); } -template -inline bool CheckedItem::isSet() const +template +inline bool CheckedItem::isSet() const { return isSet_.get(); } -template -inline void CheckedItem::reset() +template +inline void CheckedItem::reset() { isSet_.set(false); } -template -inline T& CheckedItem::set() +template +inline T & CheckedItem::set() { isSet_.set(true); return v_; } -template -inline void CheckedItem::set(bool value) +template +inline void CheckedItem::set(bool value) { isSet_.set(value); } - -template -inline void CheckedItem::setAssertMessage(std::string s) +template +inline void CheckedItem::setAssertMessage( + std::string s) { assertMsg_.set(s); } -template -inline void CheckedItem::setExceptionPtr(std::exception* e) +template +inline void CheckedItem::setExceptionPtr( + std::exception * e) { exceptionPtr_.set(e); } - - - - -template -inline IndexedMatrixT::IndexedMatrixT(const MatrixType& v,TimeIndex k): - IsSet(true), - k_(k), - v_(v) +template +inline IndexedMatrixT::IndexedMatrixT(const MatrixType & v, TimeIndex k) +: IsSet(true), k_(k), v_(v) { } -template -inline IndexedMatrixT::IndexedMatrixT(): - IsSet(false), - k_(0) +template +inline IndexedMatrixT::IndexedMatrixT() : IsSet(false), k_(0) { } ///Says whether the matrix is initialized or not -template -inline bool IndexedMatrixT::isSet()const +template +inline bool IndexedMatrixT::isSet() const { return (IsSet::get()); } ///Set the value of the matrix and the time sample -template -inline IndexedMatrixT & IndexedMatrixT::set(const MatrixType & v, TimeIndex k) +template +inline IndexedMatrixT & IndexedMatrixT::set(const MatrixType & v, + TimeIndex k) { IsSet::set(true); k_=k; @@ -190,15 +201,15 @@ inline IndexedMatrixT & IndexedMatrixT::set( } ///Switch the matrix to set -template -inline void IndexedMatrixT::set(bool b) +template +inline void IndexedMatrixT::set(bool b) { IsSet::set(b); } ///Set the value of the matrix and the time sample -template -inline void IndexedMatrixT::reset() +template +inline void IndexedMatrixT::reset() { IsSet::set(false); } @@ -207,43 +218,48 @@ inline void IndexedMatrixT::reset() ///Checks whether the matrix is set or not (assert) ///does nothing in release mode -template -inline bool IndexedMatrixT::check_() const +template +inline bool IndexedMatrixT::check_() const { BOOST_ASSERT(isSet() && "Error: Matrix not initialized, if you are initializing it, \ use set() function."); + if(isSet()) + { + BOOST_ASSERT(!v_.hasNaN() && "Error: Matrix has NaN."); + return !v_.hasNaN(); // returns false if v has a NaN : test failed + } + else + { return isSet(); + } } - - - -template -inline void IndexedMatrixT::setIndex(TimeIndex k) +template +inline void IndexedMatrixT::setIndex(TimeIndex k) { check_(); k_=k; } ///Get the matrix value -template -inline const MatrixType & IndexedMatrixT::operator()()const +template +inline const MatrixType & IndexedMatrixT::operator()() const { check_(); return v_; } ///Get the matrix value -template -inline MatrixType & IndexedMatrixT::operator()() +template +inline MatrixType & IndexedMatrixT::operator()() { check_(); return v_; } ///Get the time index -template -TimeIndex IndexedMatrixT::getTime()const +template +TimeIndex IndexedMatrixT::getTime() const { check_(); return k_; diff --git a/include/state-observation/tools/rigid-body-kinematics.hpp b/include/state-observation/tools/rigid-body-kinematics.hpp index bada8623..0013765f 100644 --- a/include/state-observation/tools/rigid-body-kinematics.hpp +++ b/include/state-observation/tools/rigid-body-kinematics.hpp @@ -82,22 +82,22 @@ inline void integrateKinematics(Vector3 & position, /// gets close to 2pi inline Vector regulateRotationVector(const Vector3 & v); -/// Transform the rotation vector into angle axis +/// Transforms the rotation vector into angle axis inline AngleAxis rotationVectorToAngleAxis(const Vector3 & v); -/// Tranbsform the rotation vector into rotation matrix +/// Transforms the rotation vector into rotation matrix inline Matrix3 rotationVectorToRotationMatrix(const Vector3 & v); -/// Tranbsform the rotation vector into quaternion +/// Transforms the rotation vector into quaternion inline Quaternion rotationVectorToQuaternion(const Vector3 & v); -/// Tranbsform the rotation matrix into rotation vector +/// Transforms the rotation matrix into rotation vector inline Vector3 rotationMatrixToRotationVector(const Matrix3 & R); /// Tranbsform a quaternion into rotation vector inline Vector3 quaternionToRotationVector(const Quaternion & q); -/// Tranbsform a quaternion into rotation vector +/// Transforms a quaternion into rotation vector inline Vector3 quaternionToRotationVector(const Vector4 & v); /// scalar component of a quaternion @@ -106,7 +106,7 @@ inline double scalarComponent(const Quaternion & q); /// vector part of the quaternion inline Vector3 vectorComponent(const Quaternion & q); -/// Transform the rotation matrix into roll pitch yaw +/// Transforms the rotation matrix into roll pitch yaw ///(decompose R into Ry*Rp*Rr) inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R, Vector3 & v); @@ -416,14 +416,25 @@ class Orientation inline Orientation inverse() const; - /// use the vector dt_x_omega as the increment of rotation expressed in the - /// world frame. Which gives R_{k+1}=\exp(S(dtxomega))R_k + /// uses the vector dt_x_omega as the increment of rotation expressed in the + /// world frame. Which gives R_{k+1}=\exp(S(dtxomega))R_k. + /// This function is also used to sum two Orientations expressed in the same frame at the same time k, even for the + /// LocalKinematics (the integration of the orientation is different but not the sum) inline const Orientation & integrate(Vector3 dt_x_omega); + /// use the vector dt_x_omega as the increment of rotation expressed in the + /// local frame. Which gives R_{k+1}=R_k*exp(S(dtxomega)) + inline const Orientation & integrateRightSide(Vector3 dt_x_omega); + /// gives the log (rotation vector) of the difference of orientation - /// gives log of (*this).inverse()*R_k1 + /// gives log of R_k1*(*this).inverse(). + /// This function is also used to differentiate two Orientations expressed in the same frame at the same time k, even + /// for the LocalKinematics (the integration of the orientation is different and therefore the associated + /// differentiation also is, but the difference remains the same) inline Vector3 differentiate(Orientation R_k1) const; + inline Vector3 differentiateRightSide(Orientation R_k1) const; + /// Rotate a vector inline Vector3 operator*(const Vector3 & v) const; @@ -440,8 +451,8 @@ class Orientation /// no checks are performed for these functions, use with caution - inline CheckedMatrix3 & getMatrixRefUnsafe(); - inline CheckedQuaternion & getQuaternionRefUnsafe(); + inline CheckedMatrix3 & getMatrixRefUnsafe() const; + inline CheckedQuaternion & getQuaternionRefUnsafe() const; /// synchronizes the representations (quaternion and rotation matrix) inline void synchronize(); @@ -464,6 +475,7 @@ class Orientation mutable CheckedMatrix3 m_; }; +struct LocalKinematics; struct Kinematics { struct Flags @@ -477,9 +489,17 @@ struct Kinematics static const Byte linAcc = BOOST_BINARY(010000); static const Byte angAcc = BOOST_BINARY(100000); + static const Byte pose = position | orientation; + static const Byte vels = linVel | angVel; + static const Byte accs = linAcc | angAcc; static const Byte all = position | orientation | linVel | angVel | linAcc | angAcc; }; + struct RecursiveAccelerationFunctorBase + { + virtual void computeRecursiveGlobalAccelerations_(Kinematics & kine) = 0; + }; + Kinematics() {} /// Constructor from a vector @@ -489,8 +509,19 @@ struct Kinematics /// use the flags to define the structure of the vector Kinematics(const Vector & v, Flags::Byte = Flags::all); + Kinematics(const CheckedVector3 & position, + const CheckedVector3 & linVel, + const CheckedVector3 & linAcc, + const Orientation & orientation, + const CheckedVector3 & angVel, + const CheckedVector3 & angAcc); + Kinematics(const Kinematics & multiplier1, const Kinematics & multiplier2); + explicit inline Kinematics(const LocalKinematics & locK); + + inline Kinematics & operator=(const LocalKinematics & locK); + /// Fills from vector /// the flags show which parts of the kinematics to be loaded from the vector /// the order of the vector is @@ -506,6 +537,8 @@ struct Kinematics Kinematics & setZero(Flags::Byte = Flags::all); + inline const Kinematics & integrateRungeKutta4(double dt, RecursiveAccelerationFunctorBase & accelerationFunctor); + inline const Kinematics & integrate(double dt); inline const Kinematics & update(const Kinematics & newValue, double dt, Flags::Byte = Flags::all); @@ -524,6 +557,12 @@ struct Kinematics inline Kinematics setToProductNoAlias(const Kinematics & operand1, const Kinematics & operand2); + /// Allows to compute the difference between two Kinematics objects. Has the same effect that calling + /// setToProductNoAlias(operand1, operand2.getInverse()) but is computationally faster + inline Kinematics setToDiffNoAlias(const Kinematics & multiplier1, const Kinematics & multiplier2); + + static inline Kinematics zeroKinematics(Flags::Byte = Flags::all); + inline void reset(); CheckedVector3 position; @@ -543,11 +582,132 @@ struct Kinematics Vector3 tempVec_; }; +struct LocalKinematics +{ + /* + This structure is similar to the Kinematics one, but all the state variables are expressed in the local frame. + (They correspond to the kinematics of the local frame in the global frame, but expressed in this local frame) + */ + struct Flags + { + typedef unsigned char Byte; + + static const Byte position = BOOST_BINARY(000001); + static const Byte orientation = BOOST_BINARY(000010); + static const Byte linVel = BOOST_BINARY(000100); + static const Byte angVel = BOOST_BINARY(001000); + static const Byte linAcc = BOOST_BINARY(010000); + static const Byte angAcc = BOOST_BINARY(100000); + + static const Byte all = position | orientation | linVel | angVel | linAcc | angAcc; + }; + + struct Derivative + { + Vector3 positionDot; + Vector3 angVel; // the rotation remains the same from local to global so the variable doesn't need to be renamed + + Vector3 linVelDot; + Vector3 angAcc; + + inline Derivative & operator=(const LocalKinematics & locKine); + }; + + struct RecursiveAccelerationFunctorBase + { + virtual void computeRecursiveLocalAccelerations_(LocalKinematics & locKine) = 0; + }; + + LocalKinematics() {} + + /// Constructor from a vector + /// the flags show which parts of the kinematics to be loaded from the vector + /// the order of the vector is + /// position orientation (quaternion) linevel angvel linAcc angAcc + /// use the flags to define the structure of the vector + inline LocalKinematics(const Vector & v, LocalKinematics::Flags::Byte flags); + + inline LocalKinematics(const LocalKinematics & multiplier1, const LocalKinematics & multiplier2); + + explicit inline LocalKinematics(const Kinematics & kin); + + inline LocalKinematics & operator=(const Kinematics & kine); + + /// Fills from vector + /// the flags show which parts of the kinematics to be loaded from the vector + /// the order of the vector is + /// position orientation (quaternion) linevel angvel linAcc angAcc + /// use the flags to define the structure of the vector + LocalKinematics & fromVector(const Vector & v, Flags::Byte = Flags::all); + + /// initializes at zero all the flagged fields + /// the typename allows to set if the prefered type for rotation + /// is a Matrix3 or a Quaternion (Quaternion by default) + template + LocalKinematics & setZero(Flags::Byte = Flags::all); + + LocalKinematics & setZero(Flags::Byte = Flags::all); + + inline const LocalKinematics & integrate(double dt); + + inline const LocalKinematics & integrateRungeKutta4(double dt, + RecursiveAccelerationFunctorBase & accelerationFunctor); + + /// updates the LocalKinematics given its new values + inline const LocalKinematics & update(const LocalKinematics & newValue, double dt, Flags::Byte = Flags::all); + + inline LocalKinematics getInverse() const; + + /// converts the object to a vector + /// the order of the vector is + /// position orientation (quaternion) linevel angvel linAcc angAcc + /// use the flags to define the structure of the vector + inline Vector toVector(Flags::Byte) const; + inline Vector toVector() const; + + /// composition of transformation + inline LocalKinematics operator*(const LocalKinematics &)const; + + inline LocalKinematics setToProductNoAlias(const LocalKinematics & operand1, const LocalKinematics & operand2); + + /// Allows to compute the difference between two LocalKinematics objects. Has the same effect that calling + /// setToProductNoAlias(operand1, operand2.getInverse()) but is computationally faster + inline LocalKinematics setToDiffNoAlias(const LocalKinematics & multiplier1, const LocalKinematics & multiplier2); + inline void reset(); + + CheckedVector3 position; // position of the frame in the destination frame of the Kinematic object, expressed in the + // original frame + Orientation orientation; + + CheckedVector3 linVel; + CheckedVector3 angVel; + + CheckedVector3 linAcc; + CheckedVector3 angAcc; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +protected: + inline const LocalKinematics & update_deprecated(const LocalKinematics & newValue, + double dt, + Flags::Byte = Flags::all); + + Vector3 tempVec_; + Vector3 tempVec_2; + Vector3 tempVec_3; + Vector3 tempVec_4; + Vector3 tempVec_5; +}; + } // namespace kine } // namespace stateObservation inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::Kinematics & k); +inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::LocalKinematics & k); + +inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::LocalKinematics::Derivative & d); + #include #endif // StATEOBSERVATIONRIGIDBODYKINEMATICS_H diff --git a/include/state-observation/tools/rigid-body-kinematics.hxx b/include/state-observation/tools/rigid-body-kinematics.hxx index 7c00bf5f..20ab7fec 100644 --- a/include/state-observation/tools/rigid-body-kinematics.hxx +++ b/include/state-observation/tools/rigid-body-kinematics.hxx @@ -140,7 +140,7 @@ inline Matrix3 rotationVectorToRotationMatrix(const Vector3 & v) return (rotationVectorToAngleAxis(Vector3(v))).toRotationMatrix(); } -/// Tranbsform the rotation vector into rotation matrix +/// Transform the rotation vector into rotation matrix inline Quaternion rotationVectorToQuaternion(const Vector3 & v) { return Quaternion(rotationVectorToAngleAxis(Vector3(v))); @@ -242,6 +242,28 @@ inline Matrix3 orthogonalizeRotationMatrix(const Matrix3 & M) return svd.matrixU() * svd.matrixV().transpose(); } +/// transform a skew symmetric 3x3 matrix into a 3d vector +inline Vector3 skewSymmetricToRotationVector(const Matrix3 & R, Vector3 & v) +{ + v(0) = R(2, 1); + v(1) = R(0, 2); + v(2) = R(1, 0); + // R << 0, -v[2], v[1], + // v[2], 0, -v[0], + // -v[1], v[0], 0; + + return v; +} + +/// transform a skew symmetric 3x3 matrix into a 3d vector +inline Vector3 skewSymmetricToRotationVector(const Matrix3 & R) +{ + Vector3 v; + skewSymmetricToRotationVector(R, v); + + return v; +} + /// transform a 3d vector into a skew symmetric 3x3 matrix inline Matrix3 skewSymmetric(const Vector3 & v, Matrix3 & R) { @@ -322,7 +344,7 @@ inline Matrix3 twoVectorsToRotationMatrix(const Vector3 & v1, const Vector3 Rv1) } else if(1 + v1dotRv1 < cst::epsilon1) { - ///it is a singularity + /// it is a singularity throw std::invalid_argument("twoVectorsToRotationMatrix Vectors are opposite: there is an infinity of rotation " "matrices with minimal angle between them"); } @@ -354,7 +376,7 @@ inline Vector3 getInvariantHorizontalVector(const Matrix3 & R) inline Vector3 getInvariantOrthogonalVector(const Matrix3 & Rhat, const Vector3 & Rtez) { Vector3 Rhat_Rtez = Rhat * Rtez; - if (Rhat_Rtez.head<2>().isZero(cst::epsilon1)) + if(Rhat_Rtez.head<2>().isZero(cst::epsilon1)) { /// any vector is a solution return Vector3::UnitX(); } @@ -849,6 +871,7 @@ inline const Orientation & Orientation::setToProductNoAlias(const Orientation & { if(R1.isMatrixSet() && R2.isMatrixSet()) { + m_.set(true); /// we set the matrix as initialized before giving the value m_.set().noalias() = R1.m_() * R2.m_(); } else @@ -862,15 +885,15 @@ inline const Orientation & Orientation::setToProductNoAlias(const Orientation & m_.set(true); /// we set the matrix as initialized before giving the value if(!R1.isMatrixSet()) { - m_().noalias() = R1.quaternionToMatrix_() * R2.m_(); + m_.getRefUnchecked().noalias() = R1.quaternionToMatrix_() * R2.m_(); } else if(!R2.isMatrixSet()) { - m_().noalias() = R1.m_() * R2.quaternionToMatrix_(); + m_.getRefUnchecked().noalias() = R1.m_() * R2.quaternionToMatrix_(); } else { - m_().noalias() = R1.m_() * R2.m_(); + m_.getRefUnchecked().noalias() = R1.m_() * R2.m_(); } q_.reset(); } @@ -908,17 +931,9 @@ inline const Orientation & Orientation::integrate(Vector3 dt_x_omega) check_(); if(q_.isSet()) { - if(isMatrixSet()) - { - Quaternion q = kine::rotationVectorToQuaternion(dt_x_omega); - q_ = q * q_(); - m_ = q.toRotationMatrix() * m_(); - } - else - { - q_ = kine::rotationVectorToQuaternion(dt_x_omega) * q_(); - } - } + m_.reset(); + q_ = kine::rotationVectorToQuaternion(dt_x_omega) * q_(); + } else { m_ = kine::rotationVectorToRotationMatrix(dt_x_omega) * m_(); @@ -926,12 +941,33 @@ inline const Orientation & Orientation::integrate(Vector3 dt_x_omega) return *this; } +inline const Orientation & Orientation::integrateRightSide(Vector3 dt_x_omega) +{ + check_(); + if(q_.isSet()) + { + m_.reset(); + q_ = q_() * kine::rotationVectorToQuaternion(dt_x_omega); + } + else + { + m_ = m_() * kine::rotationVectorToRotationMatrix(dt_x_omega); + } + return *this; +} + inline Vector3 Orientation::differentiate(Orientation R_k1) const { check_(); return (Orientation(R_k1, inverse())).toRotationVector(); } +inline Vector3 Orientation::differentiateRightSide(Orientation R_k1) const +{ + check_(); + return (Orientation(inverse(), R_k1)).toRotationVector(); +} + inline bool Orientation::isSet() const { return (isMatrixSet() || q_.isSet()); @@ -992,12 +1028,12 @@ inline Vector3 Orientation::operator*(const Vector3 & v) const return m_() * v; } -inline CheckedMatrix3 & Orientation::getMatrixRefUnsafe() +inline CheckedMatrix3 & Orientation::getMatrixRefUnsafe() const { return m_; } -inline CheckedQuaternion & Orientation::getQuaternionRefUnsafe() +inline CheckedQuaternion & Orientation::getQuaternionRefUnsafe() const { return q_; } @@ -1035,7 +1071,17 @@ inline Orientation Orientation::randomRotation() inline Kinematics::Kinematics(const Vector & v, Kinematics::Flags::Byte flags) { - fromVector(v, flags); + Kinematics::fromVector(v, flags); +} + +inline Kinematics::Kinematics(const CheckedVector3 & position, + const CheckedVector3 & linVel, + const CheckedVector3 & linAcc, + const Orientation & orientation, + const CheckedVector3 & angVel, + const CheckedVector3 & angAcc) +: position(position), linVel(linVel), linAcc(linAcc), orientation(orientation), angVel(angVel), angAcc(angAcc) +{ } inline Kinematics::Kinematics(const Kinematics & multiplier1, const Kinematics & multiplier2) @@ -1043,9 +1089,50 @@ inline Kinematics::Kinematics(const Kinematics & multiplier1, const Kinematics & setToProductNoAlias(multiplier1, multiplier2); } +inline Kinematics::Kinematics(const LocalKinematics & locK) +{ + *this = locK; +} + +inline Kinematics & Kinematics::operator=(const LocalKinematics & locK) +{ + BOOST_ASSERT(locK.orientation.isSet() && "The transformation to the local frame requires the orientation"); + + locK.orientation.toMatrix3(); + + orientation = locK.orientation; + + if(locK.position.isSet()) + { + position = orientation * locK.position(); + } + + if(locK.linVel.isSet()) + { + linVel = orientation * locK.linVel(); + } + + if(locK.linAcc.isSet()) + { + linAcc = orientation * locK.linAcc(); + } + + if(locK.angVel.isSet()) + { + angVel = orientation * locK.angVel(); + } + + if(locK.angAcc.isSet()) + { + angAcc = orientation * locK.angAcc(); + } + return *this; +} + inline Kinematics & Kinematics::fromVector(const Vector & v, Kinematics::Flags::Byte flags) { int index = 0; + reset(); bool flagPos = flags & Flags::position; bool flagLinVel = flags & Flags::linVel; @@ -1166,6 +1253,75 @@ inline Kinematics & Kinematics::setZero(Kinematics::Flags::Byte flags) return setZero(flags); } +inline const Kinematics & Kinematics::integrateRungeKutta4(double dt, RecursiveAccelerationFunctorBase & accelerationFunctor) +{ + BOOST_ASSERT((position.isSet() && orientation.isSet() && linVel.isSet() && angVel.isSet()) && "The kinematics of the LocalKinematics object are not entirely set"); + BOOST_ASSERT((linAcc.isSet() && angAcc.isSet()) && "The accelerations of the LocalKinematics object are not entirely set"); + Kinematics y234; + + Kinematics k1; // (velocities + accelerations) that have to be precomputed + Kinematics k2; + Kinematics k3; + Kinematics k4; + + k1.linVel = linVel; + k1.angVel = angVel; + k1.linAcc = linAcc; + k1.angAcc = angAcc; + + y234.position = position() + 0.5 * dt * k1.linVel; + y234.linVel = linVel() + 0.5 * dt * k1.linAcc; + + y234.orientation = orientation; + y234.orientation.integrate(0.5 * dt * k1.angVel); + y234.angVel = angVel() + 0.5 * dt * k1.angAcc; + + accelerationFunctor.computeRecursiveGlobalAccelerations_(y234); // computation of k2 (the acceleration part) in the global frame + + //conversion of k2 to the local frame in order to make them compatible with the LocalKinematics object + k2.linVel = y234.linVel; + k2.angVel = y234.angVel; + k2.linAcc = y234.linAcc; + k2.angAcc = y234.angAcc; + + y234.position = position() + 0.5 * dt * k2.linVel; + y234.linVel = linVel() + 0.5 * dt * k2.linAcc; + + y234.orientation = orientation; + y234.orientation.integrate(0.5 * dt * k2.angVel); + y234.angVel = angVel() + 0.5 * dt * k2.angAcc; + + accelerationFunctor.computeRecursiveGlobalAccelerations_(y234); // computation of k3 (the acceleration part) in the global frame + + //conversion of k3 to the local frame in order to make them compatible with the LocalKinematics object + k3.linVel = y234.linVel; + k3.angVel = y234.angVel; + k3.linAcc = y234.linAcc; + k3.angAcc = y234.angAcc; + + y234.position = position() + dt * k3.linVel; + y234.linVel = linVel() + dt * k3.linAcc; + + y234.orientation = orientation; + y234.orientation.integrate(dt * k3.angVel); + y234.angVel = angVel() + dt * k3.angAcc; + + accelerationFunctor.computeRecursiveGlobalAccelerations_(y234); // computation of k4 (the acceleration part) in the global frame + + //conversion of k4 to the local frame in order to make them compatible with the LocalKinematics object + k4.linVel = y234.linVel; + k4.angVel = y234.angVel; + k4.linAcc = y234.linAcc; + k4.angAcc = y234.angAcc; + + position() += dt/6 * (k1.linVel() + 2 * k2.linVel() + 2 * k3.linVel() + k4.linVel()); + orientation.integrate(dt / 6 * (k1.angVel() + 2 * k2.angVel() + 2 * k3.angVel() + k4.angVel())); + linVel() += dt/6 * (k1.linAcc() + 2 * k2.linAcc() + 2 * k3.linAcc() + k4.linAcc()); + angVel() += dt/6 * (k1.angAcc() + 2 * k2.angAcc() + 2 * k3.angAcc() + k4.angAcc()); + + return *this; +} + inline const Kinematics & Kinematics::integrate(double dt) { if(angVel.isSet()) @@ -1174,7 +1330,7 @@ inline const Kinematics & Kinematics::integrate(double dt) { if(orientation.isSet()) { - orientation.integrate(angVel() * dt + angAcc() * dt * dt / 2); + orientation.integrate(angVel() * dt + angAcc() * dt * dt * 0.5); } angVel() += angAcc() * dt; } @@ -1193,7 +1349,7 @@ inline const Kinematics & Kinematics::integrate(double dt) { if(position.isSet()) { - position() += linVel() * dt + linAcc() * dt * dt / 2; + position() += linVel() * dt + linAcc() * dt * dt * 0.5; } linVel() += linAcc() * dt; } @@ -1270,6 +1426,7 @@ inline const Kinematics & Kinematics::update(const Kinematics & newValue, double if(flagLinVel) { + if(newVel.isSet()) { velMethod = useVelocity; @@ -1358,23 +1515,13 @@ inline const Kinematics & Kinematics::update(const Kinematics & newValue, double } else { - if(posMethod == useVelocity) + if(posMethod == useVelocity || posMethod == useVelAndAcc ) { thisPos() += thisVel() * dt; } - else + if(posMethod == useVelAndAcc || posMethod == useAcceleration) { - if(posMethod == useVelAndAcc) - { - thisPos() += thisVel() * dt + thisAcc() * dt * dt / 2; - } - else - { - if(posMethod == useAcceleration) - { - thisPos() += thisAcc() * dt * dt / 0.5; - } - } + thisPos() += thisAcc() * dt * dt * 0.5; } } @@ -1392,7 +1539,7 @@ inline const Kinematics & Kinematics::update(const Kinematics & newValue, double if(accMethod == useAcceleration) { - thisAcc = newAcc(); + thisAcc = newAcc; } if(!flagPos) @@ -1531,13 +1678,13 @@ inline const Kinematics & Kinematics::update(const Kinematics & newValue, double { if(accMethod == useOrientation) // then velocity cannot use accelerations { - thisAcc = 2 * newOri.differentiate(thisOri) / (dt * dt); + thisAcc = 2 * thisOri.differentiate(newOri) / (dt * dt); } } if(velMethod == useOrientation) // then position cannot use velocity { - if(accMethod == useVelAndAcc) + if(accMethod == useOriAndVel) { thisAcc = -thisVel(); thisVel = thisOri.differentiate(newOri) / dt; @@ -1564,13 +1711,13 @@ inline const Kinematics & Kinematics::update(const Kinematics & newValue, double { if(posMethod == useVelAndAcc) { - thisOri.integrate(thisVel() * dt + thisAcc() * dt * dt / 2); + thisOri.integrate(thisVel() * dt + thisAcc() * dt * dt * 0.5); } else { if(posMethod == useAcceleration) { - thisOri.integrate(thisAcc() * dt * dt / 0.5); + thisOri.integrate(thisAcc() * dt * dt * 0.5); } } } @@ -1624,7 +1771,7 @@ inline Kinematics Kinematics::getInverse() const if(angAcc.isSet()) { - inverted.angAcc = r2 * (angVel().cross(angVel()) - angAcc()); // omega2dot + inverted.angAcc = -(r2 * angAcc()); // omega2dot } } @@ -1636,12 +1783,13 @@ inline Kinematics Kinematics::getInverse() const { Vector3 omegaxp = angVel().cross(position()); - inverted.linVel = r2 * (omegaxp - linVel()); // t2dot + inverted.linVel = r2.toMatrix3() * (omegaxp - linVel()); // t2dot if(linAcc.isSet() && (angAcc.isSet())) { inverted.linAcc = - r2 * (angVel().cross(2 * linVel - omegaxp) - linAcc() + angAcc().cross(position())); // t2dotdot + r2.toMatrix3() + * (angVel().cross(2 * linVel() - omegaxp) - linAcc() + angAcc().cross(position())); // t2dotdot } } } @@ -1663,10 +1811,10 @@ inline Kinematics Kinematics::setToProductNoAlias(const Kinematics & multiplier1 BOOST_ASSERT((multiplier2.position.isSet() || multiplier2.orientation.isSet()) && "The multiplier 2 kinematics is not initialized, the multiplication is not possible."); - if(multiplier2.position.isSet() && multiplier1.position.isSet()) + if(multiplier2.position.isSet()) { position.set(true); - Vector3 & R1p2 = position(); /// reference ( Vector3& ) + Vector3 & R1p2 = position.getRefUnchecked(); /// reference ( Vector3& ) R1p2.noalias() = multiplier1.orientation * multiplier2.position(); if(multiplier2.linVel.isSet() && multiplier1.linVel.isSet() && multiplier1.angVel.isSet()) @@ -1675,7 +1823,7 @@ inline Kinematics Kinematics::setToProductNoAlias(const Kinematics & multiplier1 R1p2d.noalias() = multiplier1.orientation * multiplier2.linVel(); linVel.set(true); - Vector3 & w1xR1p2 = linVel(); /// reference + Vector3 & w1xR1p2 = linVel.getRefUnchecked(); /// reference w1xR1p2.noalias() = multiplier1.angVel().cross(R1p2); Vector3 & w1xR1p2_R1p2d = w1xR1p2; /// reference ( =linVel() ) @@ -1684,7 +1832,124 @@ inline Kinematics Kinematics::setToProductNoAlias(const Kinematics & multiplier1 if(multiplier2.linAcc.isSet() && multiplier1.linAcc.isSet() && multiplier1.angAcc.isSet()) { linAcc.set(true); - linAcc().noalias() = multiplier1.orientation * multiplier2.linAcc(); + linAcc.getRefUnchecked().noalias() = multiplier1.orientation * multiplier2.linAcc(); + linAcc().noalias() += multiplier1.angAcc().cross(R1p2); + linAcc().noalias() += multiplier1.angVel().cross(w1xR1p2_R1p2d + R1p2d); + linAcc() += multiplier1.linAcc(); + } + else + { + linAcc.reset(); + } + + linVel() += multiplier1.linVel(); + } + else + { + linVel.reset(); + linAcc.reset(); + } + if (multiplier1.position.isSet()) + { + position() += multiplier1.position(); + } + else + { + position.reset(); + } + } + else + { + position.reset(); + linVel.reset(); + linAcc.reset(); + } + + if(multiplier2.orientation.isSet()) + { + orientation.setToProductNoAlias(multiplier1.orientation, multiplier2.orientation); + + if(multiplier2.angVel.isSet() && multiplier1.angVel.isSet()) + { + angVel.set(true); + Vector3 & R1w2 = angVel.getRefUnchecked(); /// reference + R1w2.noalias() = multiplier1.orientation * multiplier2.angVel(); + + if(multiplier2.angAcc.isSet() && multiplier1.angAcc.isSet()) + { + angAcc.set(true); + angAcc.getRefUnchecked().noalias() = multiplier1.orientation * multiplier2.angAcc(); + angAcc().noalias() += multiplier1.angVel().cross(R1w2); + angAcc() += multiplier1.angAcc(); + } + else + { + angAcc.reset(); + } + + angVel() += multiplier1.angVel(); + } + else + { + angVel.reset(); + angAcc.reset(); + } + } + else + { + orientation.reset(); + angVel.reset(); + angAcc.reset(); + } + + return *this; +} + +inline Kinematics Kinematics::zeroKinematics(Flags::Byte flags) +{ + Kinematics kine; + kine.setZero(flags); + return kine; +} + +inline Kinematics Kinematics::setToDiffNoAlias(const Kinematics & multiplier1, const Kinematics & multiplier2) +{ + BOOST_ASSERT(multiplier1.orientation.isSet() + && "The multiplier 1 orientation is not initialized, the multiplication is not possible."); + + BOOST_ASSERT(multiplier2.orientation.isSet() + && "The multiplier 2 orientation is not initialized, the multiplication is not possible."); + + BOOST_ASSERT((multiplier2.position.isSet()) + && "The multiplier 2 kinematics is not initialized, the multiplication is not possible."); + + Orientation R2t = multiplier2.orientation.inverse(); + if(multiplier2.position.isSet() && multiplier1.position.isSet()) + { + position.set(true); + Vector3 & R1p2 = position(); /// reference ( Vector3& ) + R1p2.noalias() = - (multiplier1.orientation * (R2t * multiplier2.position())); // -(R2t * multiplier2.position()) is the inverse of the position of multiplier 2 + + if(multiplier2.linVel.isSet() && multiplier1.linVel.isSet() && multiplier1.angVel.isSet() && multiplier2.angVel.isSet()) + { + Vector3 & R1p2d = tempVec_; /// reference + Vector3 omegaxp2 = multiplier2.angVel().cross(multiplier2.position()); + R1p2d.noalias() = multiplier1.orientation *(R2t * (omegaxp2 - multiplier2.linVel())); // R2t * (multiplier2.angVel().cross(multiplier2.position()) - multiplier2.linVel()) is the inverse of the linear velocity of multiplier 2 + + linVel.set(true); + Vector3 & w1xR1p2 = linVel(); /// reference + w1xR1p2.noalias() = multiplier1.angVel().cross(R1p2); + + Vector3 & w1xR1p2_R1p2d = w1xR1p2; /// reference ( =linVel() ) + w1xR1p2_R1p2d += R1p2d; + + if(multiplier2.linAcc.isSet() && multiplier1.linAcc.isSet() && multiplier1.angAcc.isSet() && multiplier2.angAcc.isSet()) + { + linAcc.set(true); + + linAcc().noalias() = multiplier1.orientation * + (R2t * (multiplier2.angVel().cross(2 * multiplier2.linVel() - omegaxp2) - multiplier2.linAcc() + multiplier2.angAcc().cross(multiplier2.position()))); + // R2t * (multiplier2.angVel().cross(2 * multiplier2.linVel() - omegaxp2) - multiplier2.linAcc() + multiplier2.angAcc().cross(multiplier2.position())) is the inverse of the linear velocity of multiplier 2 linAcc().noalias() += multiplier1.angAcc().cross(R1p2); linAcc().noalias() += multiplier1.angVel().cross(w1xR1p2_R1p2d + R1p2d); linAcc() += multiplier1.linAcc(); @@ -1713,18 +1978,18 @@ inline Kinematics Kinematics::setToProductNoAlias(const Kinematics & multiplier1 if(multiplier2.orientation.isSet()) { - orientation.setToProductNoAlias(multiplier1.orientation, multiplier2.orientation); + orientation.setToProductNoAlias(multiplier1.orientation, R2t); if(multiplier2.angVel.isSet() && multiplier1.angVel.isSet()) { angVel.set(true); Vector3 & R1w2 = angVel(); /// reference - R1w2.noalias() = multiplier1.orientation * multiplier2.angVel(); + R1w2.noalias() = - (multiplier1.orientation * (R2t * multiplier2.angVel())); if(multiplier2.angAcc.isSet() && multiplier1.angAcc.isSet()) { angAcc.set(true); - angAcc().noalias() = multiplier1.orientation * multiplier2.angAcc(); + angAcc().noalias() = - (multiplier1.orientation * (R2t * multiplier2.angAcc())); angAcc().noalias() += multiplier1.angVel().cross(R1w2); angAcc() += multiplier1.angAcc(); } @@ -1921,7 +2186,7 @@ inline const Kinematics & Kinematics::update_deprecated(const Kinematics & newVa position() += linVel() * dt; if(linAcc.isSet()) { - position() += linAcc() * dt * dt / 2; + position() += linAcc() * dt * dt *0.5; } } } @@ -2027,7 +2292,7 @@ inline const Kinematics & Kinematics::update_deprecated(const Kinematics & newVa increment += angVel() * dt; if(angAcc.isSet()) { - increment += angAcc() * dt * dt / 2; + increment += angAcc() * dt * dt * 0.5; } orientation.integrate(increment); } @@ -2113,42 +2378,1692 @@ inline const Kinematics & Kinematics::update_deprecated(const Kinematics & newVa return *this; } -} // namespace kine +/////////////////////////////////////////////////////////////////////// +/// -------------------LocalKinematics structure implementation------------- +/////////////////////////////////////////////////////////////////////// -} // namespace stateObservation +inline LocalKinematics::LocalKinematics(const Vector & v, LocalKinematics::Flags::Byte flags) +{ + LocalKinematics::fromVector(v, flags); +} -inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::Kinematics & k) +inline LocalKinematics::LocalKinematics(const LocalKinematics & multiplier1, const LocalKinematics & multiplier2) { - if(!k.position.isSet() && !k.orientation.isSet() && !k.linVel.isSet() && !k.angVel.isSet() && !k.linAcc.isSet() - && !k.angAcc.isSet()) + setToProductNoAlias(multiplier1, multiplier2); +} + +inline LocalKinematics::LocalKinematics(const Kinematics & kin) // we consider we can modify directly the globalKine variables + // as it would then be outdated +{ + *this = kin; +} + +inline LocalKinematics & LocalKinematics::operator=(const Kinematics & kin) +{ + BOOST_ASSERT(kin.orientation.isSet() && "The transformation to the local frame requires the orientation"); + + orientation = kin.orientation; + + kin.orientation.toMatrix3(); + + Orientation orientation_T = kin.orientation.inverse(); + + if(kin.position.isSet()) { - os << "empty kinematics" << std::endl; + position = orientation_T * kin.position(); } - if(k.position.isSet() || k.orientation.isSet()) + if(kin.linVel.isSet()) { - if(k.position.isSet()) - { - os << "pos : " << k.position().transpose() << " "; - } - else - { - os << "pos : "; - } + linVel = orientation_T * kin.linVel(); + } - if(k.orientation.isSet()) + if(kin.linAcc.isSet()) + { + linAcc = orientation_T * kin.linAcc(); + } + + if(kin.angVel.isSet()) + { + angVel = orientation_T * kin.angVel(); + } + + if(kin.angAcc.isSet()) + { + angAcc = orientation_T * kin.angAcc(); + } + return *this; +} + +inline LocalKinematics & LocalKinematics::fromVector(const Vector & v, LocalKinematics::Flags::Byte flags) +{ + int index = 0; + reset(); + + bool flagPos = flags & Flags::position; + bool flagLinVel = flags & Flags::linVel; + bool flagLinAcc = flags & Flags::linAcc; + bool flagOri = flags & Flags::orientation; + bool flagAngVel = flags & Flags::angVel; + bool flagAngAcc = flags & Flags::angAcc; + + if(flagPos) + { + BOOST_ASSERT(v.size() >= index + 3 && "The kinematics vector size is incorrect (loading position)"); + if(v.size() >= index + 3) { - os << "ori : " << k.orientation.toRotationVector().transpose() << std::endl; + position = v.segment<3>(index); + index += 3; } - else + } + + if(flagOri) + { + BOOST_ASSERT(v.size() >= index + 4 && "The kinematics vector size is incorrect (loading orientaTion)"); + if(v.size() >= index + 4) { - os << "ori :" << std::endl; + orientation.fromVector4(v.segment<4>(index)); + index += 4; } } - if(k.linVel.isSet() || k.angVel.isSet()) + if(flagLinVel) { - if(k.linVel.isSet()) + BOOST_ASSERT(v.size() >= index + 3 && "The kinematics vector size is incorrect (loading linear velocity)"); + if(v.size() >= index + 3) + { + linVel = v.segment<3>(index); + index += 3; + } + } + + if(flagAngVel) + { + BOOST_ASSERT(v.size() >= index + 3 && "The kinematics vector size is incorrect (loading angular velocity)"); + if(v.size() >= index + 3) + { + angVel = v.segment<3>(index); + index += 3; + } + } + + if(flagLinAcc) + { + BOOST_ASSERT(v.size() >= index + 3 && "The kinematics vector size is incorrect (loading linear acceleration)"); + if(v.size() >= index + 3) + { + linAcc = v.segment<3>(index); + index += 3; + } + } + + if(flagAngAcc) + { + BOOST_ASSERT(v.size() >= index + 3 && "The kinematics vector size is incorrect (loading angular acceleration)"); + if(v.size() >= index + 3) + { + angAcc = v.segment<3>(index); + // index+=3; ///useless + } + } + + return *this; +} + +template +inline LocalKinematics & LocalKinematics::setZero(LocalKinematics::Flags::Byte flags) +{ + + bool flagPos = flags & Flags::position; + bool flagLinVel = flags & Flags::linVel; + bool flagLinAcc = flags & Flags::linAcc; + bool flagOri = flags & Flags::orientation; + bool flagAngVel = flags & Flags::angVel; + bool flagAngAcc = flags & Flags::angAcc; + + if(flagPos) + { + position.set().setZero(); + } + + if(flagOri) + { + orientation.setZeroRotation(); + } + + if(flagLinVel) + { + linVel.set().setZero(); + } + + if(flagAngVel) + { + angVel.set().setZero(); + } + + if(flagLinAcc) + { + linAcc.set().setZero(); + } + + if(flagAngAcc) + { + angAcc.set().setZero(); + } + + return *this; +} + +inline LocalKinematics & LocalKinematics::setZero(LocalKinematics::Flags::Byte flags) +{ + return setZero(flags); +} + +inline const LocalKinematics & LocalKinematics::integrateRungeKutta4(double dt, RecursiveAccelerationFunctorBase & accelerationFunctor) +{ + BOOST_ASSERT((position.isSet() && orientation.isSet() && linVel.isSet() && angVel.isSet()) && "The kinematics of the LocalKinematics object are not entirely set"); + BOOST_ASSERT((linAcc.isSet() && angAcc.isSet()) && "The accelerations of the LocalKinematics object are not entirely set"); + LocalKinematics y234; + + LocalKinematics::Derivative k1; // (velocities + accelerations) that have to be precomputed + LocalKinematics::Derivative k2; + LocalKinematics::Derivative k3; + LocalKinematics::Derivative k4; + + k1 = *this; + + y234.position = position() + 0.5 * dt * k1.positionDot; + y234.linVel = linVel() + 0.5 * dt * k1.linVelDot; + + y234.orientation = orientation; + y234.orientation.integrateRightSide(0.5 * dt * k1.angVel); + y234.angVel = angVel() + 0.5 * dt * k1.angAcc; + + accelerationFunctor.computeRecursiveLocalAccelerations_(y234); // computation of k2 (the acceleration part) in the global frame + + //conversion of k2 to the local frame in order to make them compatible with the LocalKinematics object + k2 = y234; + + y234.position = position() + 0.5 * dt * k2.positionDot; + y234.linVel = linVel() + 0.5 * dt * k2.linVelDot; + + y234.orientation = orientation; + y234.orientation.integrateRightSide(0.5 * dt * k2.angVel); + y234.angVel = angVel() + 0.5 * dt * k2.angAcc; + + accelerationFunctor.computeRecursiveLocalAccelerations_(y234); // computation of k3 (the acceleration part) in the global frame + + //conversion of k3 to the local frame in order to make them compatible with the LocalKinematics object + k3 = y234; + + y234.position = position() + dt * k3.positionDot; + y234.linVel = linVel() + dt * k3.linVelDot; + + y234.orientation = orientation; + y234.orientation.integrateRightSide(dt * k3.angVel); + y234.angVel = angVel() + dt * k3.angAcc; + + accelerationFunctor.computeRecursiveLocalAccelerations_(y234); // computation of k4 (the acceleration part) in the global frame + + //conversion of k4 to the local frame in order to make them compatible with the LocalKinematics object + k4 = y234; + + position() += dt/6 * (k1.positionDot + 2 * k2.positionDot + 2 * k3.positionDot + k4.positionDot); + orientation.integrateRightSide(dt/6 * (k1.angVel + 2 * k2.angVel + 2 * k3.angVel + k4.angVel)); + linVel() += dt/6 * (k1.linVelDot + 2 * k2.linVelDot + 2 * k3.linVelDot + k4.linVelDot); + angVel() += dt/6 * (k1.angAcc + 2 * k2.angAcc + 2 * k3.angAcc + k4.angAcc); + + return *this; +} + +inline const LocalKinematics & LocalKinematics::integrate(double dt) +{ + /* + Linear part + */ + + if(linVel.isSet()) + { + if(linAcc.isSet()) + { + if(position.isSet()) + { + if(angVel.isSet()) + { + + if(angAcc.isSet()) + { + position() += dt + * (-angVel().cross(position() + dt * (linVel() - 0.5 * angVel().cross(position()))) + linVel() + + 0.5 * dt * (linAcc() - angAcc().cross(position()))); + } + else + { + position() += dt + * (-angVel().cross(position() + dt * (linVel() - 0.5 * angVel().cross(position()))) + linVel() + + 0.5 * dt * linAcc()); + } + linVel() += dt * (-angVel().cross(linVel()) + linAcc()); + } + else + { + + if(angAcc.isSet()) + { + position() += dt * (linVel() + 0.5* dt * (linAcc() - angAcc().cross(position()))); + } + else + { + position() += dt * (linVel() + 0.5 * dt * linAcc()); + } + linVel() += dt * linAcc(); + } + } + } + else + { + + if(position.isSet()) + { + if(angVel.isSet()) + { + linVel() -= angVel().cross(linVel()); + if(angAcc.isSet()) + { + position() += dt + * (-angVel().cross(position() + dt * (linVel() - 0.5 * angVel().cross(position()))) + linVel() + - 0.5 * dt * angAcc().cross(position())); + } + else + { + position() += + dt * (-angVel().cross(position() + dt * (linVel() - 0.5 * angVel().cross(position()))) + linVel()); + } + } + else + { + if(angAcc.isSet()) + { + position() += dt * (linVel() - 0.5 * dt * angAcc().cross(position())); + } + else + { + position() += dt * linVel(); + } + } + } + } + /* + Angular part + */ + + if(angVel.isSet()) + { + if(angAcc.isSet()) + { + if(orientation.isSet()) + { + orientation.integrateRightSide(angVel() * dt + angAcc() * 0.5 * dt * dt); + } + angVel() += angAcc() * dt; + } + else + { + if(orientation.isSet()) + { + orientation.integrateRightSide(angVel() * dt); + } + } + } + } + + return *this; +} + +inline const LocalKinematics & LocalKinematics::update(const LocalKinematics & newValue, double dt, Flags::Byte flags) +{ + { + bool backupAngVel = false; + bool backupAngAcc = false; + + bool flagPos = flags & Flags::position; + bool flagLinVel = flags & Flags::linVel; + bool flagLinAcc = flags & Flags::linAcc; + + bool flagOri = flags & Flags::orientation; + bool flagAngVel = flags & Flags::angVel; + bool flagAngAcc = flags & Flags::angAcc; + + CheckedVector3 & thisLinPos = position; // linear variables to be updated + CheckedVector3 & thisLinVel = linVel; + CheckedVector3 & thisLinAcc = linAcc; + + const CheckedVector3 & newLinPos = newValue.position; // new linear variables used for the update + const CheckedVector3 & newLinVel = newValue.linVel; + const CheckedVector3 & newLinAcc = newValue.linAcc; + + Orientation & thisOri = orientation; // angular variables to be updated + CheckedVector3 & thisAngVel = angVel; + CheckedVector3 & thisAngAcc = angAcc; + + CheckedVector3 currentAngVel; + CheckedVector3 currentAngAcc; + + const Orientation & newOri = newValue.orientation; // new angular variables used for the update + const CheckedVector3 & newAngVel = newValue.angVel; + const CheckedVector3 & newAngAcc = newValue.angAcc; + + enum method + { + noUpdate, + + usePosition, + useLinVelocity, + useLinAcceleration, + useLinVelAndAcc, + useLinPosAndVel, + + useOrientation, + useAngVelocity, + useAngAcceleration, + useAngVelAndAcc, + useOriAndAngVel, + + useLinVelFromPos, + useLinVelFromAngVel, + usePosFromAngVelAndAcc, + usePosFromAngVel, + usePosFromAngAcc + }; + + method posMethod = noUpdate; + method linVelMethod = noUpdate; + method linAccMethod = noUpdate; + + method oriMethod = noUpdate; + method angVelMethod = noUpdate; + method angAccMethod = noUpdate; + + bool computAngVel = false; // Even if a variable is not asked to be computed, it can be interesting to compute it + bool computAngAcc = false; + bool computPos = false; + + + + /* + Determination of the computation methods for the angular variables + */ + BOOST_ASSERT(thisOri.isSet() && "The orientation is essential to the Kinematics object"); + BOOST_ASSERT(flagOri && "The new orientation has to be computed as it is essential to the LocalKinematics object"); + if(flagOri) + { + if(newOri.isSet()) + { + oriMethod = useOrientation; + } + else + { + BOOST_ASSERT(thisOri.isSet() && "The orientation is trying to be updated without initial value"); + if(thisAngVel.isSet()) + { + if(thisAngAcc.isSet()) + { + oriMethod = useAngVelAndAcc; + } + else + { + oriMethod = useAngVelocity; + } + } + else + { + BOOST_ASSERT(thisAngAcc.isSet() && "The orientation cannot be updated with so few information"); + if(thisAngAcc.isSet()) + { + oriMethod = useAngAcceleration; + } + } + } + } + + if(newAngVel.isSet() || (thisOri.isSet() && newOri.isSet()) || (thisAngVel.isSet() && thisAngAcc.isSet())) + //if(flagAngVel) + { + computAngVel = true; + if(newAngVel.isSet()) + { + angVelMethod = useAngVelocity; + } + else + { + if(thisOri.isSet() && newOri.isSet()) + { + angVelMethod = useOrientation; + } + else + { + BOOST_ASSERT(thisAngVel.isSet() && "The angular velocity is trying to be updated without initial value"); + BOOST_ASSERT(thisAngAcc.isSet() && "The angular velocity cannot be updated with so few information"); + if(thisAngAcc.isSet()) + { + angVelMethod = useAngAcceleration; + } + } + } + } + else + { + BOOST_ASSERT(!flagAngVel && "The angular velocity cannot be updated with so few information"); + } + + + if(newAngAcc.isSet() || (thisAngVel.isSet() && newAngVel.isSet()) + || (thisAngVel.isSet() && thisOri.isSet() && newOri.isSet()) + || (thisOri.isSet() && newOri.isSet())) + //if(flagAngAcc) + { + computAngAcc = true; + if(newAngAcc.isSet()) + { + angAccMethod = useAngAcceleration; + } + else + { + if(thisAngVel.isSet() && newAngVel.isSet()) + { + angAccMethod = useAngVelocity; + } + else + { + if(thisAngVel.isSet() && angVelMethod == useOrientation) + { + angAccMethod = useOriAndAngVel; + } + else /// velocity is not available + { + if(thisOri.isSet() && newOri.isSet()) + { + angAccMethod = useOrientation; + } + else + { + BOOST_ASSERT(newAngAcc.isSet() + && "The angular accleration is trying to be updated without initial value"); + } + } + } + } + } + else + { + BOOST_ASSERT(!flagAngAcc && "The angular acceleration cannot be updated with so few information"); + } + + /* + Determination of the computation methods for the linear variables + */ + if(newLinPos.isSet() || (thisLinPos.isSet() && (thisLinVel.isSet() || thisLinAcc.isSet() || thisAngVel.isSet() || thisAngAcc.isSet()))) + //if(flagPos) + { + computPos = true; + if(newLinPos.isSet()) + { + posMethod = usePosition; + } + else + { + if(thisAngVel.isSet()) // we will need a backup of the angular velocity + { + backupAngVel = true; + } + if(thisAngAcc.isSet()) // we will need a backup of the angular acceleration + { + backupAngAcc = true; + } + BOOST_ASSERT(thisLinPos.isSet() && "The position cannot be updated without initial value"); + if(thisLinVel.isSet()) + { + if(thisLinAcc.isSet()) + { + posMethod = useLinVelAndAcc; + } + else + { + posMethod = useLinVelocity; + } + } + else + { + if(thisLinAcc.isSet()) + { + posMethod = useLinAcceleration; + } + else + { + if(thisAngVel.isSet()) + { + if(thisAngAcc.isSet()) + { + posMethod = useAngVelAndAcc; + } + else + { + posMethod = useAngVelocity; + } + } + else + { + BOOST_ASSERT(thisAngAcc.isSet() && "The position cannot be updated with so few information"); + if(thisAngAcc.isSet()) + { + posMethod = useAngAcceleration; + } + } + } + } + } + } + else + { + BOOST_ASSERT(!flagPos && "The position cannot be updated with so few information"); + + } + + + + if(newLinVel.isSet() || (thisLinPos.isSet() && computPos && posMethod != useLinVelocity && posMethod != useLinVelAndAcc) + || (thisLinVel.isSet() && (thisAngVel.isSet() || thisLinAcc.isSet()))) + //if(flagLinVel) + { + //computLinVel = true; + if(newLinVel.isSet()) + { + linVelMethod = useLinVelocity; + } + else + { + if(thisLinPos.isSet() && computPos && posMethod != useLinVelocity && posMethod != useLinVelAndAcc) + { + if(posMethod == usePosition) // means that the new position is set + { + linVelMethod = usePosition; + } + + if(posMethod == useAngVelAndAcc) + { + linVelMethod = usePosFromAngVelAndAcc; + } + if(posMethod == useAngVelocity) + { + linVelMethod = usePosFromAngVel; + } + if(posMethod == useAngAcceleration) + { + linVelMethod = usePosFromAngAcc; + } + + } + else + { + BOOST_ASSERT(thisLinVel.isSet() && "The linear velocity cannot be updated without initial value"); + if(thisAngVel.isSet()) // we will need a backup of the angular velocity + { + backupAngVel = true; + } + if(thisLinAcc.isSet()) + { + linVelMethod = useLinAcceleration; + } + else + { + BOOST_ASSERT(thisAngVel.isSet() && "The linear velocity cannot be updated with so few information"); + if(thisAngVel.isSet()) + { + linVelMethod = useAngVelocity; + } + } + }if (flagPos) + { + + } + } + } + else + { + BOOST_ASSERT(!flagLinVel && "The linear velocity cannot be updated with so few information"); + } + + if(flagLinAcc) + { + if(newLinAcc.isSet()) + { + linAccMethod = useLinAcceleration; + } + else + { + if(thisLinVel.isSet() && computAngVel && linVelMethod != useLinAcceleration) + { + if(newLinVel.isSet()) + { + linAccMethod = useLinVelocity; + } + + else + { + if(linVelMethod == usePosition) + { + linAccMethod = useLinVelFromPos; + } + if(linVelMethod == useAngVelocity) + { + linAccMethod = useLinVelFromAngVel; + } + } + + } + + else /// velocity is not available + { + BOOST_ASSERT((computAngVel && computAngAcc) && "The linear accleration requires the angular velocity and the angular acceleration to be computable"); + BOOST_ASSERT((thisLinPos.isSet() && computPos) && "The linear accleration cannot be updated with so few information"); + if(thisLinPos.isSet() && computPos && posMethod != useLinVelAndAcc && posMethod != useLinAcceleration && computAngVel && computAngAcc) + { + if(posMethod == usePosition) + { + linAccMethod = usePosition; + } + + if(posMethod == useAngVelAndAcc) + { + linAccMethod = usePosFromAngVelAndAcc; + } + if(posMethod == useAngVelocity) + { + linAccMethod = usePosFromAngVel; + } + if(posMethod == useAngAcceleration) + { + linAccMethod = usePosFromAngAcc; + } + + } + } + } + } + + /* + Computations for angular variables + */ + + if(backupAngVel) + { + currentAngVel = thisAngVel; + } + if(backupAngAcc) + { + currentAngAcc = thisAngAcc; + } + + if(angAccMethod == useAngVelocity) // then velocity cannot use accelerations + { + thisAngAcc = (newAngVel() - thisAngVel()) / dt; + } + else + { + if(angAccMethod == useOrientation) // then velocity cannot use accelerations + { + thisAngAcc = 2 * thisOri.differentiateRightSide(newOri) / (dt * dt); + } + } + + if(angVelMethod == useOrientation) // then position cannot use velocity + { + if(angAccMethod == useOriAndAngVel) + { + thisAngAcc = -thisAngVel(); + thisAngVel = thisOri.differentiateRightSide(newOri) / dt; + thisAngAcc() += thisAngVel(); + thisAngAcc() /= dt; + } + else + { + thisAngVel = thisOri.differentiateRightSide(newOri) / dt; + } + } + + if(oriMethod == useOrientation) + { + thisOri = newOri; + } + else + { + if(oriMethod == useAngVelocity) + { + thisOri.integrateRightSide(thisAngVel() * dt); + } + else + { + if(oriMethod == useAngVelAndAcc) + { + thisOri.integrateRightSide(thisAngVel() * dt + thisAngAcc() * 0.5 * dt * dt); + } + else + { + if(oriMethod == useAngAcceleration) + { + thisOri.integrateRightSide(thisAngAcc() * dt * dt * 0.5); + } + } + } + } + + if(angVelMethod == useAngVelocity) + { + thisAngVel = newAngVel; + } + else + { + if(angVelMethod == useAngAcceleration) + { + thisAngVel() += thisAngAcc() * dt; + } + } + + if(angAccMethod == useAngAcceleration) + { + thisAngAcc = newAngAcc; + } + + /* + Computations for linear variables + */ + + if(linAccMethod == useLinVelocity) // then velocity cannot use accelerations + { + thisLinAcc = thisAngVel().cross(newLinVel()) + (newLinVel() - thisLinVel()) / dt; + } + else + { + if(linAccMethod == usePosition) // then velocity cannot use accelerations + { + + thisLinAcc = 2 * (newLinPos() - thisLinPos()) / dt / dt + thisAngAcc().cross(newLinPos()) + + thisAngVel().cross(thisAngVel().cross(thisLinPos()) + + 2 * (newLinPos() - thisLinPos()) / dt); + } + + } + + + + if(linVelMethod == usePosition) // then position cannot use velocity + { + if(linAccMethod == useLinVelFromPos) // use position and velocity to get the accelerations + { + + thisLinAcc = -thisLinVel() / dt; + thisLinVel = thisAngVel().cross(newLinPos()) + (newLinPos() - thisLinPos()) / dt; + thisLinAcc() += thisAngVel().cross(thisLinVel()) + thisLinVel() / dt; + } + else + { + thisLinVel = thisAngVel().cross(newLinPos()) + (newLinPos() - thisLinPos()) / dt; + } + } + + + + if(linVelMethod == useAngVelocity) + { + if(linAccMethod == useLinVelFromAngVel) // use position and velocity to get the accelerations + { + thisLinAcc = -thisLinVel() / dt; + thisLinVel() -= dt * currentAngVel().cross(thisLinVel()); + thisLinAcc() += thisAngVel().cross(thisLinVel()) + thisLinVel() / dt; + } + else + { + thisLinVel() -= dt * currentAngVel().cross(thisLinVel()); + } + } + + if(posMethod == useAngVelAndAcc) + { + if(linVelMethod == usePosFromAngVelAndAcc || linAccMethod == usePosFromAngVelAndAcc) + { + if(linVelMethod == usePosFromAngVelAndAcc) + { + if(linAccMethod == usePosFromAngVelAndAcc) + { + thisLinAcc = -2 * (thisLinPos() + thisAngVel().cross(thisLinPos()) / dt) / dt; + thisLinVel = -thisLinPos() / dt; + thisLinPos() += dt + * (-currentAngVel().cross(thisLinPos() + dt * (-0.5 * currentAngVel().cross(thisLinPos()))) + - 0.5 * dt * currentAngAcc().cross(thisLinPos())); + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; + thisLinAcc() += thisAngAcc().cross(thisLinPos()) + + thisAngVel().cross(thisAngVel().cross(thisLinPos())) + + 2 * (thisAngVel().cross(thisLinPos()) + thisLinPos() / dt) / dt; + + } + else + { + if(linAccMethod == useLinVelFromPos) // use position and velocity to get the accelerations + { + thisLinAcc = -thisLinVel() / dt; + thisLinVel = -thisLinPos() / dt; + thisLinPos() += thisLinPos() += + dt + * (-currentAngVel().cross(thisLinPos() + dt * (-0.5 * currentAngVel().cross(thisLinPos()))) + - 0.5 * dt * currentAngAcc().cross(thisLinPos())); // newpos + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; // f(newpos) + thisLinAcc() += thisAngVel().cross(thisLinVel()) + thisLinVel() / dt; + } + else + { + thisLinVel = -thisLinPos() / dt; + thisLinPos() += + dt + * (-currentAngVel().cross(thisLinPos() + dt * (-0.5 * currentAngVel().cross(thisLinPos()))) + - 0.5 * dt * currentAngAcc().cross(thisLinPos())); + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; + } + } + + } + else // if(linAccMethod == usePosFromAngVelAndAcc) + { + thisLinAcc = -2 * (thisLinPos() + thisAngVel().cross(thisLinPos()) / dt) / dt; + thisLinPos() += dt + * (-currentAngVel().cross(thisLinPos() + dt * (-0.5 * currentAngVel().cross(thisLinPos()))) + - 0.5 * dt * currentAngAcc().cross(thisLinPos())); + thisLinAcc() += thisAngAcc().cross(thisLinPos()) + + thisAngVel().cross(thisAngVel().cross(thisLinPos())) + + 2 * (thisAngVel().cross(thisLinPos()) + thisLinPos() / dt) / dt; + } + + } + else + { + thisLinPos() += dt + * (-currentAngVel().cross(thisLinPos() + dt * (-0.5 * currentAngVel().cross(thisLinPos()))) + - 0.5 * dt * currentAngAcc().cross(thisLinPos())); + } + } + + + if(posMethod == useAngVelocity) + { + if(linVelMethod == usePosFromAngVel || linAccMethod == usePosFromAngVel) + { + if(linVelMethod == usePosFromAngVel) + { + if(linAccMethod == useLinVelFromPos) // use position and velocity to get the accelerations + { + thisLinAcc = -thisLinVel() / dt; + thisLinVel = -thisLinPos() / dt; + thisLinPos() += + dt * (-currentAngVel().cross(thisLinPos() - dt * 0.5 * currentAngVel().cross(thisLinPos()))); + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; + thisLinAcc() += thisAngVel().cross(thisLinVel()) + thisLinVel() / dt; + } + else + { + thisLinVel = -thisLinPos() / dt; + thisLinPos() += + dt * (-currentAngVel().cross(thisLinPos() - dt * 0.5 * currentAngVel().cross(thisLinPos()))); + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; + } + } + if(linAccMethod == usePosFromAngVel) + { + thisLinAcc = -2 * (thisLinPos() + thisAngVel().cross(thisLinPos()) / dt) / dt; + thisLinPos() += + -currentAngVel().cross(dt * (thisLinPos() - dt * (0.5 * currentAngVel().cross(thisLinPos())))); + thisLinAcc() += thisAngAcc().cross(thisLinPos()) + + thisAngVel().cross(thisAngVel().cross(thisLinPos())) + + 2 * (thisAngVel().cross(thisLinPos()) + thisLinPos() / dt) / dt; + } + } + else + { + thisLinPos() += -currentAngVel().cross(dt * (thisLinPos() - dt * (0.5 * currentAngVel().cross(thisLinPos())))); + } + } + + if(posMethod == useAngAcceleration) + { + if(linVelMethod == usePosFromAngAcc || linAccMethod == usePosFromAngAcc) + { + if(linVelMethod == usePosFromAngAcc) + { + if(linAccMethod == useLinVelFromPos) // use position and velocity to get the accelerations + { + thisLinAcc = -thisLinVel() / dt; + thisLinVel = -thisLinPos() / dt; + thisLinPos() -= 0.5 * dt * dt * currentAngAcc().cross(thisLinPos()); + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; + thisLinAcc() += thisAngVel().cross(thisLinVel()) + thisLinVel() / dt; + } + else + { + thisLinVel = -thisLinPos() / dt; + thisLinPos() -= 0.5 * dt * dt * currentAngAcc().cross(thisLinPos()); + thisLinVel() += thisAngVel().cross(thisLinPos()) + thisLinPos() / dt; + } + } + if(linAccMethod == usePosFromAngAcc) + { + thisLinAcc = -2 * (thisLinPos() + thisAngVel().cross(thisLinPos()) / dt) / dt; + thisLinPos() -= 0.5 * dt * dt * currentAngAcc().cross(thisLinPos()); + thisLinAcc() += thisAngAcc().cross(thisLinPos()) + thisAngVel().cross(thisAngVel().cross(thisLinPos())) + + 2 * (thisAngVel().cross(thisLinPos()) + thisLinPos() / dt) / dt; + } + } + + else + { + thisLinPos() -= 0.5 * dt * dt * currentAngAcc().cross(thisLinPos()); + } + } + + + if(posMethod == usePosition) + { + thisLinPos = newLinPos; + } + else + { + if(posMethod == useLinVelocity) + { + if(currentAngVel.isSet()) + { + if(currentAngAcc.isSet()) + { + thisLinPos() += dt + * (-currentAngVel().cross(thisLinPos() + + dt * (thisLinVel() - 0.5 * currentAngVel().cross(thisLinPos()))) + + thisLinVel() - 0.5 * dt * currentAngAcc().cross(thisLinPos())); + } + else + { + thisLinPos() += dt + * (-currentAngVel().cross(thisLinPos() + + dt * (thisLinVel() - 0.5 * currentAngVel().cross(thisLinPos()))) + + thisLinVel()); + } + } + else + { + if(currentAngAcc.isSet()) + { + thisLinPos() += dt * (thisLinVel() - 0.5 * dt * currentAngAcc().cross(thisLinPos())); + } + else + { + thisLinPos() += dt * thisLinVel(); + } + } + } + + else + { + if(posMethod == useLinVelAndAcc) + { + + if(currentAngVel.isSet()) + { + + if(currentAngAcc.isSet()) + { + + thisLinPos() += dt + * (-currentAngVel().cross( + thisLinPos() + dt * (thisLinVel() - 0.5 * currentAngVel().cross(thisLinPos()))) + + thisLinVel() + 0.5 * dt * (thisLinAcc() - currentAngAcc().cross(thisLinPos()))); + } + else + { + thisLinPos() += dt + * (-currentAngVel().cross( + thisLinPos() + dt * (thisLinVel() - 0.5 * currentAngVel().cross(thisLinPos()))) + + thisLinVel() + 0.5 * dt * (thisLinAcc())); + } + } + else + { + if(currentAngAcc.isSet()) + { + thisLinPos() += dt * (thisLinVel() + 0.5 * dt * (thisLinAcc() - currentAngAcc().cross(thisLinPos()))); + } + else + { + thisLinPos() += dt * (thisLinVel() + 0.5 * dt * thisLinAcc()); + } + } + } + else + { + if(posMethod == useLinAcceleration) + { + if(currentAngVel.isSet()) + { + if(currentAngAcc.isSet()) + { + thisLinPos() += + dt + * (-currentAngVel().cross(thisLinPos() - dt * (0.5 * currentAngVel().cross(thisLinPos()))) + + 0.5 * dt * (thisLinAcc() - currentAngAcc().cross(thisLinPos()))); + } + else + { + thisLinPos() += + dt + * (-currentAngVel().cross(thisLinPos() - dt * (0.5 * currentAngVel().cross(thisLinPos()))) + + 0.5 * dt * thisLinAcc()); + } + } + else + { + if(currentAngAcc.isSet()) + { + thisLinPos() += + 0.5 * dt * dt * (thisLinAcc() - currentAngAcc().cross(thisLinPos())); + } + else + { + thisLinPos() += 0.5 * dt * dt * thisLinAcc(); + } + } + } + } + } + } + + if(linVelMethod == useLinVelocity) + { + thisLinVel = newLinVel; + } + else + { + if(linVelMethod == useLinAcceleration) + { + if(currentAngVel.isSet()) + { + thisLinVel() += dt * (-currentAngVel().cross(thisLinVel()) + thisLinAcc()); + } + else + { + thisLinVel() += dt * thisLinAcc(); + } + } + } + + if(linAccMethod == useLinAcceleration) + { + thisLinAcc = newLinAcc; + } + + if(!flagPos) + { + thisLinPos.reset(); + } + if(!flagLinVel) + { + thisLinVel.reset(); + } + if(!flagLinAcc) + { + thisLinAcc.reset(); + } + + if(!flagOri) + { + thisOri.reset(); + } + if(!flagAngVel) + { + thisAngVel.reset(); + } + if(!flagAngAcc) + { + thisAngAcc.reset(); + } + } + return *this; +} + +inline LocalKinematics LocalKinematics::getInverse() const +{ + LocalKinematics inverted; + + BOOST_ASSERT(orientation.isSet() && "The orientation is not initialized, the kinematics cannot be inverted."); + + inverted.orientation = orientation.inverse(); + + if(angVel.isSet()) + { + inverted.angVel = -(orientation * angVel()); // omega2 + + if(angAcc.isSet()) + { + inverted.angAcc = -(orientation * angAcc()); // omega2dot + } + } + + if(position.isSet()) + { + inverted.position = -(orientation * position()); + + if(linVel.isSet() && angVel.isSet()) + { + + Vector3 omegaxp = angVel().cross(position()); + inverted.linVel = orientation * (omegaxp - linVel()); // t2dot + if(linAcc.isSet() && (angAcc.isSet())) + { + + inverted.linAcc = + orientation + * (angVel().cross(2 * linVel - omegaxp) - linAcc() + angAcc().cross(position())); // t2dotdot + } + } + } + + return inverted; +} + +inline LocalKinematics LocalKinematics::operator*(const LocalKinematics & multiplier) const +{ + return LocalKinematics(*this, multiplier); +} + +inline LocalKinematics LocalKinematics::setToProductNoAlias(const LocalKinematics & multiplier1, + const LocalKinematics & multiplier2) +{ + BOOST_ASSERT(multiplier1.orientation.isSet() + && "The multiplier 1 orientation is not initialized, the multiplication is not possible."); + BOOST_ASSERT(multiplier2.orientation.isSet() + && "The multiplier 2 orientation is not initialized, the multiplication is not possible."); + + BOOST_ASSERT((multiplier2.position.isSet() || multiplier2.orientation.isSet()) + && "The multiplier 2 kinematics is not initialized, the multiplication is not possible."); + + Orientation R1t = multiplier1.orientation.inverse(); + Orientation R2t = multiplier2.orientation.inverse(); + + if(multiplier2.position.isSet() && multiplier1.position.isSet()) + { + position.set(true); + Vector3 & R2tp1 = position.getRefUnchecked(); /// reference ( Vector3& ) + R2tp1.noalias() = R2t * multiplier1.position(); + + if(multiplier2.linVel.isSet() && multiplier1.linVel.isSet() && multiplier1.angVel.isSet()) + { + Vector3 & R2tp1d = tempVec_; /// reference + R2tp1d.noalias() = R2t * multiplier1.linVel(); + + Vector3 & R2tw1 = tempVec_2; /// reference + R2tw1.noalias() = R2t * multiplier1.angVel(); + + linVel.set(true); + + Vector3 & R2tw1p2 = linVel.getRefUnchecked(); /// reference + R2tw1p2.noalias() = R2tw1.cross(multiplier2.position()); + + Vector3 & R2tw1p2_p2d = R2tw1p2; /// reference ( =linVel() ) + R2tw1p2_p2d += multiplier2.linVel(); + + if(multiplier2.linAcc.isSet() && multiplier1.linAcc.isSet() && multiplier1.angAcc.isSet()) + { + linAcc.set(true); + linAcc.getRefUnchecked().noalias() = R2t * multiplier1.linAcc(); + linAcc().noalias() += (R2t * multiplier1.angAcc()).cross(multiplier2.position()); + linAcc().noalias() += R2tw1.cross(R2tw1p2_p2d + multiplier2.linVel()); + linAcc() += multiplier2.linAcc(); + } + else + { + linAcc.reset(); + } + + linVel() += R2tp1d; + } + else + { + linVel.reset(); + linAcc.reset(); + } + + position() += multiplier2.position(); + } + else + { + position.reset(); + linVel.reset(); + linAcc.reset(); + } + + if(multiplier2.orientation.isSet()) + { + orientation.setToProductNoAlias(multiplier1.orientation, multiplier2.orientation); + + if(multiplier2.angVel.isSet() && multiplier1.angVel.isSet()) + { + angVel.set(true); + Vector3 & R2tw1 = angVel.getRefUnchecked(); /// reference + R2tw1.noalias() = R2t * multiplier1.angVel(); + + if(multiplier2.angAcc.isSet() && multiplier1.angAcc.isSet()) + { + angAcc.set(true); + angAcc.getRefUnchecked().noalias() = R2t * multiplier1.angAcc(); + angAcc().noalias() += R2tw1.cross(multiplier2.angVel()); + angAcc() += multiplier2.angAcc(); + } + else + { + angAcc.reset(); + } + + angVel() += multiplier2.angVel(); + } + else + { + angVel.reset(); + angAcc.reset(); + } + } + else + { + orientation.reset(); + angVel.reset(); + angAcc.reset(); + } + + return *this; +} + +inline LocalKinematics LocalKinematics::setToDiffNoAlias(const LocalKinematics & multiplier1, + const LocalKinematics & multiplier2) +{ + BOOST_ASSERT(multiplier1.orientation.isSet() + && "The multiplier 1 orientation is not initialized, the multiplication is not possible."); + BOOST_ASSERT(multiplier2.orientation.isSet() + && "The multiplier 2 orientation is not initialized, the multiplication is not possible."); + + BOOST_ASSERT((multiplier2.position.isSet()) + && "The multiplier 2 kinematics is not initialized, the multiplication is not possible."); + + Orientation R1t = multiplier1.orientation.inverse(); + Orientation R2t = multiplier2.orientation; // transpose of the inversed orientation of multiplier 2 + + if(multiplier2.position.isSet() && multiplier1.position.isSet()) + { + position.set(true); + Vector3 & R2tp1 = position.getRefUnchecked(); /// reference ( Vector3& ) + + Vector3 & inv_pos2 = tempVec_3; // inversed position of multiplier 2 + inv_pos2.noalias() = -(multiplier2.orientation*multiplier2.position()); + R2tp1.noalias() = R2t * multiplier1.position(); + + if(multiplier1.linVel.isSet() && multiplier2.linVel.isSet() && multiplier1.angVel.isSet() && multiplier2.angVel.isSet()) + { + Vector3 & R2tp1d = tempVec_; /// reference + R2tp1d.noalias() = R2t * multiplier1.linVel(); + + Vector3 & R2tw1 = tempVec_2; /// reference + R2tw1.noalias() = R2t * multiplier1.angVel(); + + linVel.set(true); + + Vector3 & R2tw1p2 = linVel.getRefUnchecked(); /// reference + R2tw1p2.noalias() = R2tw1.cross(inv_pos2); + + Vector3 & R2tw1p2_p2d = R2tw1p2; /// reference ( =linVel() ) + + Vector3 & inv_linVel2 = tempVec_4; // inversed linear velocity of multiplier 2 + inv_linVel2.noalias() = multiplier2.orientation * (multiplier2.angVel().cross(multiplier2.position()) - multiplier2.linVel()); + + R2tw1p2_p2d += inv_linVel2; + + if(multiplier1.linAcc.isSet() && multiplier2.linAcc.isSet() && multiplier1.angAcc.isSet() && multiplier2.angAcc.isSet()) + { + linAcc.set(true); + linAcc.getRefUnchecked().noalias() = R2t * multiplier1.linAcc(); + linAcc().noalias() += (R2t * multiplier1.angAcc()).cross(inv_pos2); + linAcc().noalias() += R2tw1.cross(R2tw1p2_p2d + inv_linVel2); + + Vector3 & inv_linAcc2 = tempVec_5; // inversed linear acceleration of multiplier 2 + inv_linAcc2.noalias() = multiplier2.orientation + *(multiplier2.angVel().cross(2*multiplier2.linVel()-multiplier2.angVel().cross(multiplier2.position())) - multiplier2.linAcc() + multiplier2.angAcc().cross(multiplier2.position())); + + linAcc() += inv_linAcc2; + } + else + { + linAcc.reset(); + } + + linVel() += R2tp1d; + } + else + { + linVel.reset(); + linAcc.reset(); + } + + position() += inv_pos2; + } + else + { + position.reset(); + linVel.reset(); + linAcc.reset(); + } + + if(multiplier2.orientation.isSet()) + { + orientation.setToProductNoAlias(multiplier1.orientation, multiplier2.orientation.inverse()); + + if(multiplier2.angVel.isSet() && multiplier1.angVel.isSet()) + { + angVel.set(true); + Vector3 & R2tw1 = angVel.getRefUnchecked(); /// reference + R2tw1.noalias() = R2t * multiplier1.angVel(); + + if(multiplier2.angAcc.isSet() && multiplier1.angAcc.isSet()) + { + angAcc.set(true); + angAcc.getRefUnchecked().noalias() = R2t * multiplier1.angAcc(); + angAcc().noalias() -= R2tw1.cross(multiplier2.orientation * multiplier2.angVel()); // multiplier2.orientation * multiplier2.angVel() is the inverse of the angular velocity of multiplier 2 + angAcc().noalias() -= multiplier2.orientation * multiplier2.angAcc(); // multiplier2.orientation * multiplier2.angAcc() is the inverse of the angular acceleration of multiplier 2 + } + else + { + angAcc.reset(); + } + + angVel().noalias() -= multiplier2.orientation * multiplier2.angVel(); // multiplier2.orientation * multiplier2.angVel() is the inverse of the angular velocity of multiplier 2 + } + else + { + angVel.reset(); + angAcc.reset(); + } + } + else + { + orientation.reset(); + angVel.reset(); + angAcc.reset(); + } + + return *this; +} + +inline Vector LocalKinematics::toVector(Flags::Byte flags) const +{ + int size = 0; + + if(flags & Flags::position) + { + size += 3; + } + if(flags & Flags::orientation) + { + size += 4; + } + if(flags & Flags::linVel) + { + size += 3; + } + if(flags & Flags::angVel) + { + size += 3; + } + if(flags & Flags::linAcc) + { + size += 3; + } + if(flags & Flags::angAcc) + { + size += 3; + } + + Vector output(size); + + int curIndex = 0; + if((flags & Flags::position)) + { + output.segment<3>(curIndex) = position(); + curIndex += 3; + } + if((flags & Flags::orientation)) + { + output.segment<4>(curIndex) = orientation.toVector4(); + curIndex += 4; + } + if((flags & Flags::linVel)) + { + output.segment<3>(curIndex) = linVel(); + curIndex += 3; + } + if((flags & Flags::angVel)) + { + output.segment<3>(curIndex) = angVel(); + curIndex += 3; + } + if((flags & Flags::linAcc)) + { + output.segment<3>(curIndex) = linAcc(); + curIndex += 3; + } + if((flags & Flags::angAcc)) + { + output.segment<3>(curIndex) = angAcc(); + } + + return output; +} + +inline Vector LocalKinematics::toVector() const +{ + int size = 0; + if(position.isSet()) + { + size += 3; + } + if(orientation.isSet()) + { + size += 4; + } + if(linVel.isSet()) + { + size += 3; + } + if(angVel.isSet()) + { + size += 3; + } + if(linAcc.isSet()) + { + size += 3; + } + if(angAcc.isSet()) + { + size += 3; + } + + Vector output(size); + + int curIndex = 0; + if(position.isSet()) + { + output.segment<3>(curIndex) = position(); + curIndex += 3; + } + if(orientation.isSet()) + { + output.segment<4>(curIndex) = orientation.toQuaternion().coeffs(); + curIndex += 4; + } + if(linVel.isSet()) + { + output.segment<3>(curIndex) = linVel(); + curIndex += 3; + } + if(angVel.isSet()) + { + output.segment<3>(curIndex) = angVel(); + curIndex += 3; + } + if(linAcc.isSet()) + { + output.segment<3>(curIndex) = linAcc(); + curIndex += 3; + } + if(angAcc.isSet()) + { + output.segment<3>(curIndex) = angAcc(); + } + + return output; +} + +inline void LocalKinematics::reset() +{ + position.reset(); + orientation.reset(); + linVel.reset(); + angVel.reset(); + linAcc.reset(); + angAcc.reset(); +} + +/////////////////////////////////////////////////////////////////////// +/// -------------------Derivative structure implementation------------- +/////////////////////////////////////////////////////////////////////// + +inline LocalKinematics::Derivative & LocalKinematics::Derivative::operator=(const LocalKinematics & locKine) +{ + positionDot = locKine.linVel() - locKine.angVel().cross(locKine.position()); + angVel = locKine.angVel(); + linVelDot = locKine.linAcc() - locKine.angVel().cross(locKine.linVel()); + angAcc = locKine.angAcc(); + + return *this; +} + +} // namespace kine + +} // namespace stateObservation + +inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::Kinematics & k) +{ + if(!k.position.isSet() && !k.orientation.isSet() && !k.linVel.isSet() && !k.angVel.isSet() && !k.linAcc.isSet() + && !k.angAcc.isSet()) + { + os << "empty kinematics" << std::endl; + } + + if(k.position.isSet() || k.orientation.isSet()) + { + if(k.position.isSet()) + { + os << "pos : " << k.position().transpose() << " "; + } + else + { + os << "pos : "; + } + + if(k.orientation.isSet()) + { + os << "ori : " << k.orientation.toRotationVector().transpose() << std::endl; + } + else + { + os << "ori :" << std::endl; + } + } + + if(k.linVel.isSet() || k.angVel.isSet()) + { + if(k.linVel.isSet()) + { + os << "linVel: " << k.linVel().transpose() << " "; + } + else + { + os << "linVel: "; + } + + if(k.angVel.isSet()) + { + os << "angVel: " << k.angVel().transpose() << std::endl; + } + else + { + os << "angVel:" << std::endl; + } + } + + if(k.linAcc.isSet() || k.angAcc.isSet()) + { + if(k.linAcc.isSet()) + { + os << "linAcc: " << k.linAcc().transpose() << " "; + } + else + { + os << "linAcc: "; + } + + if(k.angAcc.isSet()) + { + os << "angAcc: " << k.angAcc().transpose() << std::endl; + } + else + { + os << "angAcc:" << std::endl; + } + } + + return os; +} + +inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::LocalKinematics & k) +{ + if(!k.position.isSet() && !k.orientation.isSet() && !k.linVel.isSet() && !k.angVel.isSet() + && !k.linAcc.isSet() && !k.angAcc.isSet()) + { + os << "empty kinematics" << std::endl; + } + + if(k.position.isSet() || k.orientation.isSet()) + { + if(k.position.isSet()) + { + os << "pos : " << k.position().transpose() << " "; + } + else + { + os << "pos : "; + } + + if(k.orientation.isSet()) + { + os << "ori : " << k.orientation.toRotationVector().transpose() << std::endl; + } + else + { + os << "ori :" << std::endl; + } + } + + if(k.linVel.isSet() || k.angVel.isSet()) + { + if(k.linVel.isSet()) { os << "linVel: " << k.linVel().transpose() << " "; } @@ -2190,3 +4105,16 @@ inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine return os; } + +inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::LocalKinematics::Derivative & d) +{ + os << "positionDot : " << d.positionDot.transpose() << " "; + + os << "angVel: " << d.angVel.transpose() << std::endl; + + os << "linVelDot: " << d.linVelDot.transpose() << " "; + + os << "angAcc: " << d.angAcc.transpose() << std::endl; + + return os; +} \ No newline at end of file diff --git a/src/extended-kalman-filter.cpp b/src/extended-kalman-filter.cpp index 79e7cde6..d9ca48dd 100644 --- a/src/extended-kalman-filter.cpp +++ b/src/extended-kalman-filter.cpp @@ -1,3 +1,4 @@ +#include #include namespace stateObservation @@ -151,7 +152,6 @@ KalmanFilterBase::Amatrix // ExtendedKalmanFilter::Amatrix does not work opt.x_ = this->x_(); opt.dx_.resize(nt_); - if(p_ > 0) { if(directInputStateProcessFeedthrough_) @@ -159,10 +159,8 @@ KalmanFilterBase::Amatrix // ExtendedKalmanFilter::Amatrix does not work else opt.u_ = inputVectorZero(); } - for(Index i = 0; i < nt_; ++i) { - opt.dx_.setZero(); opt.dx_[i] = dx[i]; @@ -200,7 +198,6 @@ KalmanFilterBase::Cmatrix ExtendedKalmanFilter::getCMatrixFD(const Vector & dx) arithm_->stateSum(xbar_(), opt.dx_, opt.xp_); opt.yp_ = simulateSensor_(opt.xp_, k + 1); - opt.yp_ -= ybar_(); opt.yp_ /= dx[i]; diff --git a/src/imu-elastic-local-frame-dynamical-system.cpp b/src/imu-elastic-local-frame-dynamical-system.cpp index 3523fd4e..f8328063 100644 --- a/src/imu-elastic-local-frame-dynamical-system.cpp +++ b/src/imu-elastic-local-frame-dynamical-system.cpp @@ -314,14 +314,6 @@ void IMUElasticLocalFrameDynamicalSystem::computeElastContactForcesAndMoments( -Kfv_ * op_.Rcit * op_.Rt * (kine::skewSymmetric(angVel) * op_.RciContactPos + linVelocity + orientation * op_.contactVel); - // std::cout << "RciContactPos " << op_.RciContactPos.transpose() <(3 * i) = op_.forcei; op_.momenti.noalias() = -Kte_ * op_.Rcit * op_.Rt * oriVector; @@ -369,12 +361,6 @@ void IMUElasticLocalFrameDynamicalSystem::computeElastPendulumForcesAndMoments(c if(printed_ == false) { - // std::cout << "globalContactPos=" << globalContactPos.transpose() << std::endl; - // std::cout << "ropeLength=" << ropeLength << std::endl; - // std::cout << "rope deformation=" << modifiedRopeLength-ropeLength << std::endl; - // std::cout << "contactOriUnitVector=" << contactOriUnitVector.transpose() << std::endl; - // std::cout << "forcei=" << forcei.transpose() << std::endl; - // std::cout << "momenti=" << momenti.transpose() << std::endl; printed_ = true; } } @@ -877,9 +863,6 @@ Vector IMUElasticLocalFrameDynamicalSystem::stateDynamics(const Vector & x, cons for(unsigned i = 0; i < hrp2::contact::nbModeledMax; ++i) { - // std::cout << "Contact " << i << std::endl - // << fc_.segment<3>(3*i).transpose() << std::endl - // << tc_.segment<3>(3*i).transpose()<< std::endl; op_.efforts[i].block<3, 1>(0, 0) = fc_.segment<3>(3 * i); op_.efforts[i].block<3, 1>(3, 0) = tc_.segment<3>(3 * i); } @@ -1133,9 +1116,6 @@ stateObservation::Matrix IMUElasticLocalFrameDynamicalSystem::measureDynamicsJac } } - // std::cout << "JACOBIAN: "< #ifndef NDEBUG -//#define VERBOUS_KALMANFILTER +// #define VERBOUS_KALMANFILTER #endif #ifdef VERBOUS_KALMANFILTER @@ -120,22 +120,30 @@ ObserverBase::StateVector KalmanFilterBase::oneStepEstimation_() // prediction updateStateAndMeasurementPrediction(); // runs also updatePrediction_(); - oc_.pbar.noalias() = q_ + a_ * (pr_ * a_.transpose()); + + oc_.pbar.resize(nt_, nt_); + oc_.pbar.triangularView() = q_; + oc_.pbar.triangularView() += a_ * pr_.selfadjointView() * a_.transpose(); // innovation Measurements arithm_->measurementDifference(this->y_[k + 1], ybar_(), oc_.inoMeas); - oc_.inoMeasCov.noalias() = r_ + c_ * (oc_.pbar * c_.transpose()); + + oc_.inoMeasCov.resize(mt_, mt_); + oc_.inoMeasCov.triangularView() = r_; + oc_.inoMeasCov.triangularView() += c_ * oc_.pbar.selfadjointView() * c_.transpose(); Index & measurementTangentSize = mt_; // inversing innovation measurement covariance matrix - oc_.inoMeasCovLLT.compute(oc_.inoMeasCov); + oc_.inoMeasCovLLT.compute(oc_.inoMeasCov.selfadjointView()); oc_.inoMeasCovInverse.resize(measurementTangentSize, measurementTangentSize); oc_.inoMeasCovInverse.setIdentity(); oc_.inoMeasCovLLT.matrixL().solveInPlace(oc_.inoMeasCovInverse); oc_.inoMeasCovLLT.matrixL().transpose().solveInPlace(oc_.inoMeasCovInverse); // innovation - oc_.kGain.noalias() = oc_.pbar * (c_.transpose() * oc_.inoMeasCovInverse); + + oc_.kGain.noalias() = oc_.pbar.selfadjointView() * (c_.transpose() * oc_.inoMeasCovInverse); + innovation_.noalias() = oc_.kGain * oc_.inoMeas; // update @@ -147,8 +155,10 @@ ObserverBase::StateVector KalmanFilterBase::oneStepEstimation_() std::cout << "A" << std::endl << a_.format(CleanFmt) << std::endl; std::cout << "C" << std::endl << c_.format(CleanFmt) << std::endl; std::cout << "P" << std::endl << pr_.format(CleanFmt) << std::endl; + std::cout << "Q" << std::endl << q_.format(CleanFmt) << std::endl; + std::cout << "R" << std::endl << r_.format(CleanFmt) << std::endl; std::cout << "K" << std::endl << oc_.kGain.format(CleanFmt) << std::endl; - std::cout << "Xbar" << std::endl << xbar().transpose().format(CleanFmt) << std::endl; + std::cout << "Xbar" << std::endl << xbar_().transpose().format(CleanFmt) << std::endl; std::cout << "inoMeasCov" << std::endl << oc_.inoMeasCov.format(CleanFmt) << std::endl; std::cout << "oc_.pbar" << std::endl << (oc_.pbar).format(CleanFmt) << std::endl; std::cout << "c_ * (oc_.pbar * c_.transpose())" << std::endl @@ -156,24 +166,24 @@ ObserverBase::StateVector KalmanFilterBase::oneStepEstimation_() std::cout << "inoMeasCovInverse" << std::endl << oc_.inoMeasCovInverse.format(CleanFmt) << std::endl; std::cout << "predictedMeasurement " << std::endl << ybar_().transpose().format(CleanFmt) << std::endl; std::cout << "inoMeas" << std::endl << oc_.inoMeas.transpose().format(CleanFmt) << std::endl; - std::cout << "inovation_" << std::endl << inovation_.transpose().format(CleanFmt) << std::endl; + std::cout << "inovation_" << std::endl << innovation_.transpose().format(CleanFmt) << std::endl; std::cout << "Xhat" << std::endl << oc_.xhat.transpose().format(CleanFmt) << std::endl; #endif // VERBOUS_KALMANFILTER this->x_.set(oc_.xhat, k + 1); - pr_.noalias() = -oc_.kGain * c_; - pr_.diagonal().array() += 1; - pr_ *= oc_.pbar; - // simmetrize the pr_ matrix - pr_ = (pr_ + pr_.transpose()) * 0.5; + oc_.mKc.noalias() = -oc_.kGain * c_; + oc_.mKc.diagonal().array() += 1; + + pr_.resize(nt_, nt_); + pr_.triangularView() = (oc_.mKc * oc_.pbar.selfadjointView()).eval(); return oc_.xhat; } KalmanFilterBase::Pmatrix KalmanFilterBase::getStateCovariance() const { - return pr_; + return pr_.selfadjointView(); } void KalmanFilterBase::reset() @@ -399,6 +409,11 @@ Vector KalmanFilterBase::getLastPredictedMeasurement() const return ybar_(); } +Vector KalmanFilterBase::getLastMeasurement() const +{ + return y_.back(); +} + Matrix KalmanFilterBase::getLastGain() const { return oc_.kGain; @@ -414,4 +429,9 @@ void KalmanFilterBase::setStateArithmetics(StateVectorArithmetics * a) arithm_ = a; } +void KalmanFilterBase::resetPrediction() +{ + xbar_.set(false); +} + } // namespace stateObservation diff --git a/src/kinetics-observer.cpp b/src/kinetics-observer.cpp index a312c017..c11b62f4 100644 --- a/src/kinetics-observer.cpp +++ b/src/kinetics-observer.cpp @@ -7,7 +7,7 @@ #include -#ifndef NDEBUG +#ifdef NDEBUG # include #endif @@ -54,9 +54,9 @@ const double KineticsObserver::stateOriInitVarianceDefault = 1e-4; const double KineticsObserver::stateLinVelInitVarianceDefault = 1e-6; const double KineticsObserver::stateAngVelInitVarianceDefault = 1e-6; const double KineticsObserver::gyroBiasInitVarianceDefault = 1e-10; -const double KineticsObserver::unmodeledWrenchInitVarianceDefault = 1e100; -const double KineticsObserver::contactForceInitVarianceDefault = 1e100; -const double KineticsObserver::contactTorqueInitVarianceDefault = 1e100; +const double KineticsObserver::unmodeledWrenchInitVarianceDefault = 1e2; +const double KineticsObserver::contactForceInitVarianceDefault = 1e2; +const double KineticsObserver::contactTorqueInitVarianceDefault = 1e2; const double KineticsObserver::statePoseProcessVarianceDefault = 1e-8; const double KineticsObserver::stateOriProcessVarianceDefault = 1e-8; @@ -90,10 +90,11 @@ KineticsObserver::KineticsObserver(unsigned maxContacts, unsigned maxNumberOfIMU : maxContacts_(maxContacts), maxImuNumber_(maxNumberOfIMU), contacts_(maxContacts_), imuSensors_(maxImuNumber_), stateSize_(sizeStateBase + maxImuNumber_ * sizeGyroBias + maxContacts * sizeContact), stateTangentSize_(sizeStateTangentBase + maxImuNumber_ * sizeGyroBias + sizeContactTangent * maxContacts), - measurementSize_(0), measurementTangentSize_(0), stateVector_(stateSize_), stateVectorDx_(stateTangentSize_), - oldStateVector_(stateSize_), additionalForce_(Vector3::Zero()), additionalTorque_(Vector3::Zero()), + measurementSize_(0), measurementTangentSize_(0), worldCentroidStateVector_(stateSize_), + worldCentroidStateVectorDx_(stateTangentSize_), oldWorldCentroidStateVector_(stateSize_), + additionalForce_(Vector3::Zero()), additionalTorque_(Vector3::Zero()), ekf_(stateSize_, stateTangentSize_, measurementSizeBase, measurementSizeBase, inputSize, false, false), - finiteDifferencesJacobians_(true), withGyroBias_(true), withUnmodeledWrench_(false), + finiteDifferencesJacobians_(true), withRungeKutta_(false), withGyroBias_(false), withUnmodeledWrench_(false), withAccelerationEstimation_(false), k_est_(0), k_data_(0), mass_(defaultMass), dt_(defaultdx), processNoise_(0x0), measurementNoise_(0x0), numberOfContactRealSensors_(0), currentIMUSensorNumber_(0), linearStiffnessMatDefault_(Matrix3::Identity() * linearStiffnessDefault), @@ -130,10 +131,13 @@ KineticsObserver::KineticsObserver(unsigned maxContacts, unsigned maxNumberOfIMU ekf_.setFunctor(this); ekf_.setStateArithmetics(this); - stateVector_.setZero(); - oldStateVector_ = stateVector_; + /* The initialization of the observer is now called by MCKineticsObserver + worldCentroidStateVector_.setZero(); + oldWorldCentroidStateVector_ = worldCentroidStateVector_; - ekf_.setState(stateVector_, k_est_); + ekf_.setState(worldCentroidStateVector_, k_est_); + updateKine_(); + */ stateKinematicsInitCovMat_.setZero(); stateKinematicsInitCovMat_.block(posIndexTangent(), posIndexTangent()) = statePosInitCovMat_; @@ -175,9 +179,7 @@ KineticsObserver::KineticsObserver(unsigned maxContacts, unsigned maxNumberOfIMU resetStateCovarianceMat(); resetProcessCovarianceMat(); - updateKine_(); - - stateVectorDx_.setConstant(1e-6); + worldCentroidStateVectorDx_.setConstant(1e-6); } KineticsObserver::~KineticsObserver() {} @@ -187,25 +189,27 @@ Index KineticsObserver::getStateSize() const return stateSize_; } +Index KineticsObserver::getStateTangentSize() const +{ + return stateTangentSize_; +} + Index KineticsObserver::getMeasurementSize() const { Index size = 0; - if(k_est_ != k_data_) // test if there are new measurements + for(VectorIMUConstIterator i = imuSensors_.begin(); i != imuSensors_.end(); ++i) { - for(VectorIMUConstIterator i = imuSensors_.begin(); i != imuSensors_.end(); ++i) + if(i->time == k_data_) { - if(i->time == k_data_) - { - size += sizeIMUSignal; - } + size += sizeIMUSignal; } + } - size += numberOfContactRealSensors_ * sizeWrench; + size += numberOfContactRealSensors_ * sizeWrench; - if(absPoseSensor_.time == k_data_) - { - size += sizePose; - } + if(absPoseSensor_.time == k_data_) + { + size += sizePose; } return size; @@ -226,185 +230,226 @@ void KineticsObserver::setMass(double m) mass_ = m; } -const Vector & KineticsObserver::update() +void KineticsObserver::updateMeasurements() { - if(k_est_ != k_data_) + for(VectorContactIterator i = contacts_.begin(), ie = contacts_.end(); i != ie; ++i) { - for(VectorContactIterator i = contacts_.begin(), ie = contacts_.end(); i != ie; ++i) + if(i->isSet) { - if(i->isSet) - { - BOOST_ASSERT((i->time == k_data_) && "The contacts have not all been updated. \ + BOOST_ASSERT((i->time == k_data_) && "The contacts have not all been updated. \ Either remove lost contacts using removeContact \ or Run updateContactWithWrenchSensor or updateContactWithNoSensor on every existing contact"); - /// the following code is only an attempt to maintain a consistent state of the state observer - /// therefore we unset the state - if(i->time != k_data_) + /// the following code is only an attempt to maintain a consistent state of the state observer + /// therefore we unset the state + if(i->time != k_data_) + { + if(i->withRealSensor) { - if(i->withRealSensor) - { - i->withRealSensor = false; - numberOfContactRealSensors_--; - } - i->isSet = false; + i->withRealSensor = false; + numberOfContactRealSensors_--; } + i->isSet = false; } } + } - ///////////// initialize the measurement Vector and matrix ////////////// + ///////////// initialize the measurement Vector and matrix ////////////// - measurementSize_ = sizeIMUSignal * currentIMUSensorNumber_ + sizeWrench * numberOfContactRealSensors_; - measurementTangentSize_ = measurementSize_; - if(absPoseSensor_.time == k_data_) - { - measurementSize_ += sizePose; - measurementTangentSize_ += sizePoseTangent; - } + measurementSize_ = sizeIMUSignal * currentIMUSensorNumber_ + sizeWrench * numberOfContactRealSensors_; + measurementTangentSize_ = measurementSize_; + if(absPoseSensor_.time == k_data_) + { + measurementSize_ += sizePose; + measurementTangentSize_ += sizePoseTangent; + } - measurementVector_.resize(measurementSize_); - measurementCovMatrix_.resize(measurementTangentSize_, measurementTangentSize_); - measurementCovMatrix_.setZero(); + measurementVector_.resize(measurementSize_); + measurementCovMatrix_.resize(measurementTangentSize_, measurementTangentSize_); + measurementCovMatrix_.setZero(); - int curMeasIndex = 0; + int curMeasIndex = 0; - for(VectorIMUIterator i = imuSensors_.begin(), ie = imuSensors_.end(); i != ie; ++i) + for(VectorIMUIterator i = imuSensors_.begin(), ie = imuSensors_.end(); i != ie; ++i) + { + if(i->time == k_data_) { - if(i->time == k_data_) - { - i->measIndex = curMeasIndex; - measurementVector_.segment(curMeasIndex) = i->acceleroGyro; - measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = - i->covMatrixAccelero; - curMeasIndex += sizeAcceleroSignal; - measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = i->covMatrixGyro; - curMeasIndex += sizeGyroSignal; - } + i->measIndex = curMeasIndex; + measurementVector_.segment(curMeasIndex) = i->acceleroGyro; + measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = + i->covMatrixAccelero; + curMeasIndex += sizeAcceleroSignal; + measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = i->covMatrixGyro; + curMeasIndex += sizeGyroSignal; } + } - for(VectorContactIterator i = contacts_.begin(), ie = contacts_.end(); i != ie; ++i) + for(VectorContactIterator i = contacts_.begin(), ie = contacts_.end(); i != ie; ++i) + { + if(i->withRealSensor) { - if(i->withRealSensor) - { - i->measIndex = curMeasIndex; - measurementVector_.segment(curMeasIndex) = i->wrench; - measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = i->sensorCovMatrix(); - curMeasIndex += sizeWrench; - } + i->measIndex = curMeasIndex; + measurementVector_.segment(curMeasIndex) = i->wrenchMeasurement; + measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = i->sensorCovMatrix(); + curMeasIndex += sizeWrench; } + } - if(absPoseSensor_.time == k_data_) - { - absPoseSensor_.measIndex = curMeasIndex; - BOOST_ASSERT(absPoseSensor_.pose.position.isSet() && absPoseSensor_.pose.orientation.isSet() - && "The absolute pose needs to contain the position and the orientation"); - measurementVector_.segment(curMeasIndex) = absPoseSensor_.pose.toVector(flagsPoseKine); - measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = - absPoseSensor_.covMatrix(); - } + if(absPoseSensor_.time == k_data_) + { + absPoseSensor_.measIndex = curMeasIndex; + BOOST_ASSERT(absPoseSensor_.pose.position.isSet() && absPoseSensor_.pose.orientation.isSet() + && "The absolute pose needs to contain the position and the orientation"); + measurementVector_.segment(curMeasIndex) = absPoseSensor_.pose.toVector(flagsPoseKine); + measurementCovMatrix_.block(curMeasIndex, curMeasIndex) = + absPoseSensor_.covMatrix(); + } + + ekf_.setMeasureSize(measurementSize_, measurementTangentSize_); + ekf_.setMeasurement(measurementVector_, k_data_); + ekf_.setR(measurementCovMatrix_); +} + +const Vector & KineticsObserver::update() +{ + if(k_est_ != k_data_) + { + updateMeasurements(); + + ekf_.updateStateAndMeasurementPrediction(); - ekf_.setMeasureSize(measurementSize_, measurementTangentSize_); - ekf_.setMeasurement(measurementVector_, k_data_); - ekf_.setR(measurementCovMatrix_); if(finiteDifferencesJacobians_) { - ekf_.setA(ekf_.getAMatrixFD(stateVectorDx_)); - ekf_.setC(ekf_.getCMatrixFD(stateVectorDx_)); + ekf_.setA(ekf_.getAMatrixFD(worldCentroidStateVectorDx_)); + ekf_.setC(ekf_.getCMatrixFD(worldCentroidStateVectorDx_)); } else { - ekf_.setA(computeAMatrix_()); - ekf_.setC(computeCMatrix_()); + ekf_.setA(computeAMatrix()); + ekf_.setC(computeCMatrix()); } - stateVector_ = ekf_.getEstimatedState(k_data_); + worldCentroidStateVector_ = ekf_.getEstimatedState(k_data_); - if(stateVector_.hasNaN()) + if(worldCentroidStateVector_.hasNaN()) { #ifndef NDEBUG - std::cout << "Kinetics observer: NaN value detected" << std::endl; + // std::cout << "Kinetics observer: NaN value detected" << std::endl; #endif - stateVector_ = stateNaNCorrection_(); + stateNaNCorrection_(); } else { - oldStateVector_ = stateVector_; + oldWorldCentroidStateVector_ = worldCentroidStateVector_; } ++k_est_; // the timestamp of the state we estimated - stateKinematics_.reset(); - - updateKine_(); + worldCentroidStateKinematics_.reset(); + // update of worldCentroidStateKinematics_ and of the contacts pose with the newly estimated state + updateLocalKineAndContacts_(); if(withAccelerationEstimation_) { + // update of worldCentroidStateKinematics_ with the accelerations estimateAccelerations(); } + updateGlobalKine_(); } - return stateVector_; + return worldCentroidStateVector_; } const Vector & KineticsObserver::getCurrentStateVector() const { - return stateVector_; + return worldCentroidStateVector_; } -stateObservation::TimeIndex KineticsObserver::getStateVectorTimeIndex() const +const stateObservation::TimeIndex KineticsObserver::getStateVectorTimeIndex() const { return ekf_.getCurrentTime(); } -kine::Kinematics KineticsObserver::getKinematics() const +kine::LocalKinematics KineticsObserver::getLocalCentroidKinematics() const { - return stateKinematics_; + return worldCentroidStateKinematics_; } -kine::Kinematics KineticsObserver::getKinematicsOf(const Kinematics & local) const +kine::LocalKinematics KineticsObserver::getLocalKinematicsOf(const LocalKinematics & locKin) const { - return Kinematics(stateKinematics_, local); /// product of the kinematics + return LocalKinematics(worldCentroidStateKinematics_, locKin); /// product of the kinematics +} + +/* Care : unsafe access, for the moment the global kinematics are updated only after the update, so don't call this + * function before*/ +kine::Kinematics KineticsObserver::getGlobalCentroidKinematics() const +{ + return worldCentroidKinematics_; +} + +kine::Kinematics KineticsObserver::getGlobalKinematicsOf(const Kinematics & userBodyKin) const +{ + Kinematics centroidBodyKine = userBodyKin; + if(centroidBodyKine.position.isSet()) + { + centroidBodyKine.position() -= com_(); + } + if(centroidBodyKine.linVel.isSet()) + { + centroidBodyKine.linVel() -= comd_(); + } + if(centroidBodyKine.linAcc.isSet()) + { + centroidBodyKine.linAcc() -= comdd_(); + } + + return Kinematics(Kinematics(worldCentroidStateKinematics_), + centroidBodyKine); /// product of the kinematics -> worldBodyKine } Vector6 KineticsObserver::getContactWrench(int contactNbr) const { - return stateVector_.segment(contactWrenchIndex(contactNbr)); + return worldCentroidStateVector_.segment(contactWrenchIndex(contactNbr)); } kine::Kinematics KineticsObserver::getContactPosition(int contactNbr) const { - return Kinematics(stateVector_.segment(contactKineIndex(contactNbr)), flagsContactKine); + return Kinematics(worldCentroidStateVector_.segment(contactKineIndex(contactNbr)), flagsContactKine); } Vector6 KineticsObserver::getUnmodeledWrench() const { - return stateVector_.segment(unmodeledWrenchIndex()); + return worldCentroidStateVector_.segment(unmodeledWrenchIndex()); } -kine::Kinematics KineticsObserver::estimateAccelerations() +kine::LocalKinematics KineticsObserver::estimateAccelerations() { - Vector3 forceLocal = additionalForce_; - Vector3 torqueLocal = additionalTorque_; + Vector3 forceCentroid = additionalForce_; + Vector3 torqueCentroid = additionalTorque_; - addUnmodeledAndContactWrench_(stateVector_, forceLocal, torqueLocal); + addUnmodeledAndContactWrench_(worldCentroidStateVector_, forceCentroid, torqueCentroid); /// The accelerations are about to be computed so we set them to "initialized" - stateKinematics_.linAcc.set(true); - stateKinematics_.angAcc.set(true); + worldCentroidStateKinematics_.linAcc.set(true); + worldCentroidStateKinematics_.angAcc.set(true); - computeAccelerations_(stateKinematics_, forceLocal, torqueLocal, stateKinematics_.linAcc(), - stateKinematics_.angAcc()); + computeLocalAccelerations_(worldCentroidStateKinematics_, forceCentroid, torqueCentroid, + worldCentroidStateKinematics_.linAcc(), worldCentroidStateKinematics_.angAcc()); - return stateKinematics_; + return worldCentroidStateKinematics_; } -void KineticsObserver::setStateKinematics(const Kinematics & kine, bool resetForces, bool resetCovariance) +void KineticsObserver::setWorldCentroidStateKinematics(const LocalKinematics & localKine, + bool resetForces, + bool resetCovariance) { - BOOST_ASSERT(kine.position.isSet() && kine.orientation.isSet() && kine.linVel.isSet() && kine.angVel.isSet() + BOOST_ASSERT(localKine.position.isSet() && localKine.orientation.isSet() && localKine.linVel.isSet() + && localKine.angVel.isSet() && "The Kinematics is not correctly initialized, should be the position, orientation, and linear and " "angular verlocities"); - stateKinematics_ = kine; - stateVector_.segment(kineIndex()) = stateKinematics_.toVector(flagsStateKine); + worldCentroidStateKinematics_ = localKine; + worldCentroidStateVector_.segment(kineIndex()) = + worldCentroidStateKinematics_.toVector(flagsStateKine); if(resetForces) { @@ -412,12 +457,12 @@ void KineticsObserver::setStateKinematics(const Kinematics & kine, bool resetFor { if(i->isSet) { - stateVector_.segment(contactWrenchIndex(i)).setZero(); + worldCentroidStateVector_.segment(contactWrenchIndex(i)).setZero(); } } } - ekf_.setState(stateVector_, k_est_); + ekf_.setState(worldCentroidStateVector_, k_est_); if(resetCovariance) { @@ -438,10 +483,31 @@ void KineticsObserver::setStateKinematics(const Kinematics & kine, bool resetFor } } +void KineticsObserver::setWorldCentroidStateKinematics(const Kinematics & kine, bool resetCovariance) +{ + LocalKinematics localKine = LocalKinematics(kine); + BOOST_ASSERT(kine.position.isSet() && kine.orientation.isSet() && kine.linVel.isSet() && kine.angVel.isSet() + && "The Kinematics is not correctly initialized, should be the position, orientation, and linear and " + "angular verlocities"); + worldCentroidStateKinematics_ = localKine; + worldCentroidStateVector_.segment(kineIndex()) = + worldCentroidStateKinematics_.toVector(flagsStateKine); + + ekf_.setState(worldCentroidStateVector_, k_est_); + + if(resetCovariance) + { + Matrix stateCovariance = ekf_.getStateCovariance(); + setBlockStateCovariance(stateCovariance, stateKinematicsInitCovMat_, kineIndex()); + + ekf_.setStateCovariance(stateCovariance); + } +} + void KineticsObserver::setGyroBias(const Vector3 & bias, unsigned numberOfIMU, bool resetCovariance) { - stateVector_.segment(gyroBiasIndex(numberOfIMU)) = bias; - ekf_.setState(stateVector_, k_est_); + worldCentroidStateVector_.segment(gyroBiasIndex(numberOfIMU)) = bias; + ekf_.setState(worldCentroidStateVector_, k_est_); if(resetCovariance) { @@ -454,8 +520,8 @@ void KineticsObserver::setGyroBias(const Vector3 & bias, unsigned numberOfIMU, b void KineticsObserver::setStateUnmodeledWrench(const Vector6 & wrench, bool resetCovariance) { - stateVector_.segment(unmodeledWrenchIndex()) = wrench; - ekf_.setState(stateVector_, k_est_); + worldCentroidStateVector_.segment(unmodeledWrenchIndex()) = wrench; + ekf_.setState(worldCentroidStateVector_, k_est_); if(resetCovariance) { @@ -468,21 +534,31 @@ void KineticsObserver::setStateUnmodeledWrench(const Vector6 & wrench, bool rese void KineticsObserver::setStateVector(const Vector & v, bool resetCovariance) { - stateVector_ = v; + worldCentroidStateVector_ = v; ekf_.setState(v, k_est_); - updateKine_(); + + updateLocalKineAndContacts_(); if(resetCovariance) { resetStateCovarianceMat(); } + oldWorldCentroidStateVector_ = worldCentroidStateVector_; } -void KineticsObserver::setAdditionalWrench(const Vector3 & force, const Vector3 & moment) +void KineticsObserver::setAdditionalWrench(const Vector3 & forceUserFrame, const Vector3 & momentUserFrame) { + startNewIteration_(); + convertWrenchFromUserToCentroid(forceUserFrame, momentUserFrame, additionalForce_, additionalTorque_); +} - additionalForce_ = force; - additionalTorque_ = moment; +void KineticsObserver::convertWrenchFromUserToCentroid(const Vector3 & forceUserFrame, + const Vector3 & momentUserFrame, + Vector3 & forceCentroidFrame, + Vector3 & momentCentroidFrame) +{ + forceCentroidFrame = forceUserFrame; + momentCentroidFrame = momentUserFrame - com_().cross(forceUserFrame); } void KineticsObserver::setWithUnmodeledWrench(bool b) @@ -495,14 +571,27 @@ void KineticsObserver::setWithAccelerationEstimation(bool b) withAccelerationEstimation_ = b; } +void KineticsObserver::useRungeKutta(bool b) +{ + withRungeKutta_ = b; +} + +bool KineticsObserver::getWithAccelerationEstimation() const +{ + return withAccelerationEstimation_; +} + void KineticsObserver::setWithGyroBias(bool b) { - withAccelerationEstimation_ = b; + withGyroBias_ = b; } -int KineticsObserver::setIMU(const Vector3 & accelero, const Vector3 & gyrometer, const Kinematics & localKine, int num) +int KineticsObserver::setIMU(const Vector3 & accelero, + const Vector3 & gyrometer, + const Kinematics & userImuKinematics, + int num) { - /// ensure the measuements are labeled with the good time stamp + /// ensure the measurements are labeled with the good time stamp startNewIteration_(); if(num < 0) @@ -520,31 +609,40 @@ int KineticsObserver::setIMU(const Vector3 & accelero, const Vector3 & gyrometer BOOST_ASSERT(imu.time < k_data_ && "The IMU has been already set, use another number"); + imu.num = num; imu.acceleroGyro.head<3>() = accelero; imu.acceleroGyro.tail<3>() = gyrometer; if(imuSensors_[num].time == 0) /// this is the first value for the IMU { + imu.userImuKinematics = userImuKinematics; + imu.centroidImuKinematics = LocalKinematics(convertUserToCentroidFrame_(imu.userImuKinematics, k_data_)); + imu.covMatrixAccelero = acceleroCovMatDefault_; imu.covMatrixGyro = gyroCovMatDefault_; - imu.kinematics = localKine; - BOOST_ASSERT(imu.kinematics.position.isSet() && imu.kinematics.orientation.isSet() + + BOOST_ASSERT(imu.centroidImuKinematics.position.isSet() && imu.centroidImuKinematics.orientation.isSet() && "The kinematics of the IMU is incorrectly initialized"); - if(!imu.kinematics.linVel.isSet()) + if(!imu.centroidImuKinematics.linVel.isSet()) { - imu.kinematics.linVel.set().setZero(); + imu.centroidImuKinematics.linVel.set().setZero(); } - if(!imu.kinematics.angVel.isSet()) + if(!imu.centroidImuKinematics.angVel.isSet()) { - imu.kinematics.angVel.set().setZero(); + imu.centroidImuKinematics.angVel.set().setZero(); } - if(!imu.kinematics.linAcc.isSet()) + if(!imu.centroidImuKinematics.linAcc.isSet()) { - imu.kinematics.linAcc.set().setZero(); + imu.centroidImuKinematics.linAcc.set().setZero(); + } + if(!imu.centroidImuKinematics.angAcc.isSet()) + { + imu.centroidImuKinematics.angAcc.set().setZero(); } } else { - imu.kinematics.update(localKine, dt_ * (k_data_ - k_data_), flagsIMUKine); + imu.userImuKinematics.update(userImuKinematics, dt_ * (k_data_ - imu.time), flagsIMUKine); + imu.centroidImuKinematics = LocalKinematics(convertUserToCentroidFrame_(imu.userImuKinematics, k_data_)); } imu.time = k_data_; @@ -557,7 +655,7 @@ int KineticsObserver::setIMU(const Vector3 & accelero, const Vector3 & gyrometer, const Matrix3 & acceleroCov, const Matrix3 & gyroCov, - const Kinematics & localKine, + const Kinematics & userImuKinematics, int num) { /// ensure the measuements are labeled with the good time stamp @@ -578,6 +676,7 @@ int KineticsObserver::setIMU(const Vector3 & accelero, BOOST_ASSERT(imu.time < k_data_ && "The IMU has been already set, use another number"); + imu.num = num; imu.acceleroGyro.head<3>() = accelero; imu.acceleroGyro.tail<3>() = gyrometer; imu.covMatrixAccelero = acceleroCov; @@ -585,25 +684,31 @@ int KineticsObserver::setIMU(const Vector3 & accelero, if(imuSensors_[num].time == 0) /// this is the first value for the IMU { - imu.kinematics = localKine; - BOOST_ASSERT(imu.kinematics.position.isSet() && imu.kinematics.orientation.isSet() + imu.userImuKinematics = userImuKinematics; + imu.centroidImuKinematics = LocalKinematics(convertUserToCentroidFrame_(imu.userImuKinematics, k_data_)); + BOOST_ASSERT(imu.centroidImuKinematics.position.isSet() && imu.centroidImuKinematics.orientation.isSet() && "The kinematics of the IMU is incorrectly initialized"); - if(!imu.kinematics.linVel.isSet()) + if(!imu.centroidImuKinematics.linVel.isSet()) + { + imu.centroidImuKinematics.linVel.set().setZero(); + } + if(!imu.centroidImuKinematics.angVel.isSet()) { - imu.kinematics.linVel.set().setZero(); + imu.centroidImuKinematics.angVel.set().setZero(); } - if(!imu.kinematics.angVel.isSet()) + if(!imu.centroidImuKinematics.linAcc.isSet()) { - imu.kinematics.angVel.set().setZero(); + imu.centroidImuKinematics.linAcc.set().setZero(); } - if(!imu.kinematics.linAcc.isSet()) + if(!imu.centroidImuKinematics.angAcc.isSet()) { - imu.kinematics.linAcc.set().setZero(); + imu.centroidImuKinematics.angAcc.set().setZero(); } } else { - imu.kinematics.update(localKine, dt_ * (k_data_ - k_data_), flagsIMUKine); + imu.userImuKinematics.update(userImuKinematics, dt_ * (k_data_ - imu.time), flagsIMUKine); + imu.centroidImuKinematics = LocalKinematics(convertUserToCentroidFrame_(imu.userImuKinematics, k_data_)); } imu.time = k_data_; @@ -619,8 +724,8 @@ void KineticsObserver::setIMUDefaultCovarianceMatrix(const Matrix3 & acceleroCov gyroCovMatDefault_ = gyroCov; } -void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrench, - const Kinematics & localKine, +void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrenchMeasurement, + const Kinematics & userContactKine, unsigned contactNumber) { /// ensure the measuements are labeled with the good time stamp @@ -633,13 +738,19 @@ void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrench, if(contacts_[contactNumber].time == k_data_ - 1) /// the contact is not newly set { - contacts_[contactNumber].localKine.update(localKine, dt_, Contact::localKineFlags); + contacts_[contactNumber].userContactKine.update(userContactKine, dt_, Contact::contactKineFlags); + convertUserToCentroidFrame_(contacts_[contactNumber].userContactKine, contacts_[contactNumber].centroidContactKine, + k_data_); + // we convert the contact's kinematics from the user frame to the centroid's frame } else /// the contact is newlyset { - contacts_[contactNumber].localKine = localKine; + contacts_[contactNumber].userContactKine = userContactKine; + convertUserToCentroidFrame_(contacts_[contactNumber].userContactKine, contacts_[contactNumber].centroidContactKine, + k_data_); + // we convert the contact's kinematics from the user frame to the centroid's frame } - contacts_[contactNumber].wrench = wrench; + contacts_[contactNumber].wrenchMeasurement = wrenchMeasurement; contacts_[contactNumber].time = k_data_; if(!contacts_[contactNumber].sensorCovMatrix.isSet()) @@ -654,9 +765,9 @@ void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrench, } } -void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrench, +void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrenchMeasurement, const Matrix6 & wrenchCovMatrix, - const Kinematics & localKine, + const Kinematics & userContactKine, unsigned contactNumber) { /// ensure the measuements are labeled with the good time stamp @@ -669,13 +780,19 @@ void KineticsObserver::updateContactWithWrenchSensor(const Vector6 & wrench, if(contacts_[contactNumber].time == k_data_ - 1) /// the contact is not newly set { - contacts_[contactNumber].localKine.update(localKine, dt_, Contact::localKineFlags); + contacts_[contactNumber].userContactKine.update(userContactKine, dt_, Contact::contactKineFlags); + convertUserToCentroidFrame_(contacts_[contactNumber].userContactKine, contacts_[contactNumber].centroidContactKine, + k_data_); + // we convert the contact's kinematics from the user frame to the centroid's frame } else /// the contact is newlyset { - contacts_[contactNumber].localKine = localKine; + contacts_[contactNumber].userContactKine = userContactKine; + convertUserToCentroidFrame_(contacts_[contactNumber].userContactKine, contacts_[contactNumber].centroidContactKine, + k_data_); + // we convert the contact's kinematics from the user frame to the centroid's frame } - contacts_[contactNumber].wrench = wrench; + contacts_[contactNumber].wrenchMeasurement = wrenchMeasurement; contacts_[contactNumber].time = k_data_; contacts_[contactNumber].sensorCovMatrix = wrenchCovMatrix; @@ -691,7 +808,7 @@ void KineticsObserver::setContactWrenchSensorDefaultCovarianceMatrix(const Matri contactWrenchSensorCovMatDefault_ = wrenchSensorCovMat; } -void KineticsObserver::updateContactWithNoSensor(const Kinematics & localKine, unsigned contactNumber) +void KineticsObserver::updateContactWithNoSensor(const Kinematics & userContactKine, unsigned contactNumber) { /// ensure the measuements are labeled with the good time stamp startNewIteration_(); @@ -703,11 +820,17 @@ void KineticsObserver::updateContactWithNoSensor(const Kinematics & localKine, u if(contacts_[contactNumber].time == k_data_ - 1) /// the contact is not newly set { - contacts_[contactNumber].localKine.update(localKine, dt_, Contact::localKineFlags); + contacts_[contactNumber].userContactKine.update(userContactKine, dt_, Contact::contactKineFlags); + convertUserToCentroidFrame_(contacts_[contactNumber].userContactKine, contacts_[contactNumber].centroidContactKine, + k_data_); + // we convert the contact's kinematics from the user frame to the centroid's frame } else /// the contact is newlyset { - contacts_[contactNumber].localKine = localKine; + contacts_[contactNumber].userContactKine = userContactKine; + convertUserToCentroidFrame_(contacts_[contactNumber].userContactKine, contacts_[contactNumber].centroidContactKine, + k_data_); + // we convert the contact's kinematics from the user frame to the centroid's frame } contacts_[contactNumber].time = k_data_; @@ -749,14 +872,14 @@ void KineticsObserver::setAbsolutePoseSensorDefaultCovarianceMatrix(const Matrix absPoseSensorCovMatDefault_ = newdefault; } -void KineticsObserver::setInertiaMatrix(const Matrix3 & I, const Matrix3 & I_dot) +void KineticsObserver::setCoMInertiaMatrix(const Matrix3 & I, const Matrix3 & I_dot) { startNewIteration_(); I_.set(I, k_data_); Id_.set(I_dot, k_data_); } -void KineticsObserver::setInertiaMatrix(const Matrix3 & I) +void KineticsObserver::setCoMInertiaMatrix(const Matrix3 & I) { startNewIteration_(); @@ -767,7 +890,7 @@ void KineticsObserver::setInertiaMatrix(const Matrix3 & I) I_.set(I, k_data_); } -void KineticsObserver::setInertiaMatrix(const Vector6 & Iv, const Vector6 & Iv_dot) +void KineticsObserver::setCoMInertiaMatrix(const Vector6 & Iv, const Vector6 & Iv_dot) { startNewIteration_(); @@ -780,7 +903,7 @@ void KineticsObserver::setInertiaMatrix(const Vector6 & Iv, const Vector6 & Iv_d fillSymmetricMatrix(Id_(), Iv_dot.head<3>(), Iv_dot(3), Iv_dot(4), Iv_dot(5)); } -void KineticsObserver::setInertiaMatrix(const Vector6 & Iv) +void KineticsObserver::setCoMInertiaMatrix(const Vector6 & Iv) { startNewIteration_(); namespace t = tools; @@ -837,14 +960,14 @@ void KineticsObserver::setCenterOfMass(const Vector3 & com) com_.set(com, k_data_); } -void KineticsObserver::setAngularMomentum(const Vector3 & sigma, const Vector3 & sigma_dot) +void KineticsObserver::setCoMAngularMomentum(const Vector3 & sigma, const Vector3 & sigma_dot) { startNewIteration_(); sigma_.set(sigma, k_data_); sigmad_.set(sigma_dot, k_data_); } -void KineticsObserver::setAngularMomentum(const Vector3 & sigma) +void KineticsObserver::setCoMAngularMomentum(const Vector3 & sigma) { startNewIteration_(); if(sigma_.getTime() < k_data_) @@ -854,8 +977,7 @@ void KineticsObserver::setAngularMomentum(const Vector3 & sigma) sigma_.set(sigma, k_data_); } -int KineticsObserver::addContact(const Kinematics & pose, - const Vector6 & contactWrench, +int KineticsObserver::addContact(const Kinematics & worldContactRefKine, const Matrix12 & initialCovarianceMatrix, const Matrix12 & processCovarianceMatrix, int contactNumber, @@ -865,9 +987,11 @@ int KineticsObserver::addContact(const Kinematics & pose, const Matrix3 & angularDamping) { - BOOST_ASSERT(pose.position.isSet() && pose.orientation.isSet() + BOOST_ASSERT(worldContactRefKine.position.isSet() && worldContactRefKine.orientation.isSet() && "The added contact pose is not initialized correctly (position and orientation)"); + /// attributes the contact an index called contactNumber. Automatically attributes the first available number in the + /// range defined by the maximum amount of contacts if(contactNumber < 0) { contactNumber = 0; @@ -882,8 +1006,8 @@ int KineticsObserver::addContact(const Kinematics & pose, && "Trying to add contact: The contact number exceeds the maximum allowed, please give a number of " "contact between 0 and maxContact-1"); - if(unsigned(contactNumber) >= maxContacts_) /// this is a bug-prone protection code that is here only to guarantee the - /// consistence of the state + /// this is a bug-prone protection code that is here only to guarantee the consistence of the state + if(unsigned(contactNumber) >= maxContacts_) { contactNumber = maxContacts_ - 1; } @@ -893,12 +1017,12 @@ int KineticsObserver::addContact(const Kinematics & pose, Contact & contact = contacts_[contactNumber]; /// reference + contact.num = contactNumber; contact.isSet = true; /// set the contacts contact.stateIndex = contactsIndex() + contactNumber * sizeContact; contact.stateIndexTangent = contactsIndexTangent() + contactNumber * sizeContactTangent; - - contact.absPose = pose; - contact.wrench = contactWrench; + contact.worldRestPose = worldContactRefKine; + contact.worldRefPose_DEBUG = worldContactRefKine; if(linearDamping != Matrix3::Constant(-1)) { @@ -936,8 +1060,11 @@ int KineticsObserver::addContact(const Kinematics & pose, contact.angularDamping = angularDampingMatDefault_; } - /// update the state vector - stateVector_.segment(contact.stateIndex) << pose.toVector(flagsContactKine), Vector6::Zero(); + /// update the state vector. The contact forces and moments are initialized to zero but are updated in update() + worldCentroidStateVector_.segment(contact.stateIndex) + << contact.worldRestPose.toVector(flagsContactKine), + Vector6::Zero(); + ekf_.setState(worldCentroidStateVector_, k_est_); /// sets the initial covariance matrix Matrix stateCovMat = ekf_.getStateCovariance(); @@ -953,21 +1080,20 @@ int KineticsObserver::addContact(const Kinematics & pose, } /// version when the contact position is perfectly known -int KineticsObserver::addContact(const Kinematics & pose, - const Vector6 & contactWrench, +int KineticsObserver::addContact(const Kinematics & worldContactRefKine, int contactNumber, const Matrix3 & linearStiffness, const Matrix3 & linearDamping, const Matrix3 & angularStiffness, const Matrix3 & angularDamping) { - return addContact(pose, contactWrench, contactInitCovMatDefault_, contactProcessCovMatDefault_, contactNumber, + return addContact(worldContactRefKine, contactInitCovMatDefault_, contactProcessCovMatDefault_, contactNumber, linearStiffness, linearDamping, angularStiffness, angularDamping); } void KineticsObserver::removeContact(int contactNbr) { - BOOST_ASSERT(!contacts_[contactNbr].isSet && "Tried to remove a non-existing contact."); + BOOST_ASSERT(contacts_[contactNbr].isSet && "Tried to remove a non-existing contact."); contacts_[contactNbr].isSet = false; if(contacts_[contactNbr].withRealSensor) { @@ -987,6 +1113,20 @@ Index KineticsObserver::getNumberOfContacts() const return contacts_.size(); } +Index KineticsObserver::getNumberOfSetContacts() const +{ + std::vector v; + + for(unsigned i = 0; i < contacts_.size(); ++i) + { + if(contacts_[i].isSet) + { + v.push_back(i); + } + } + return v.size(); +} + std::vector KineticsObserver::getListOfContacts() const { std::vector v; @@ -1106,7 +1246,7 @@ Vector KineticsObserver::getMeasurementVector() { if(i->time == k_data_ && i->withRealSensor) { - measurement.segment(currIndex) = i->wrench; + measurement.segment(currIndex) = i->wrenchMeasurement; currIndex += sizeWrench; } } @@ -1272,7 +1412,7 @@ void KineticsObserver::resetInputs() void KineticsObserver::setFiniteDifferenceStep(const Vector & v) { - stateVectorDx_ = v; + worldCentroidStateVectorDx_ = v; } void KineticsObserver::useFiniteDifferencesJacobians(bool b) @@ -1280,11 +1420,36 @@ void KineticsObserver::useFiniteDifferencesJacobians(bool b) finiteDifferencesJacobians_ = b; } -Vector KineticsObserver::stateNaNCorrection_() +void KineticsObserver::setStateContact(const int & index, + Kinematics worldContactRestKine, + const Vector6 & wrench, + bool resetCovariance) +{ + Contact contact = contacts_[index]; + + BOOST_ASSERT(contact.isSet && "The contact is currently not set"); + worldCentroidStateVector_.segment(contactPosIndex(index)) = + worldContactRestKine.toVector().segment(0); + + worldCentroidStateVector_.segment(contactWrenchIndex(index)) = wrench; + + contact.worldRestPose.fromVector(worldCentroidStateVector_.segment(contactPosIndex(index)), + flagsContactKine); + + if(resetCovariance) + { + resetStateContactCovMat(index); + } + + ekf_.setState(worldCentroidStateVector_, k_est_); +} + +void KineticsObserver::stateNaNCorrection_() { + nanDetected_ = true; + /// TODO implement this function assert(false && "NaN Correction not yet implemented. Please Contact mehdi.benallegue@gmail.com"); - return oldStateVector_; } void KineticsObserver::startNewIteration_() @@ -1294,6 +1459,15 @@ void KineticsObserver::startNewIteration_() ++k_data_; numberOfContactRealSensors_ = 0; currentIMUSensorNumber_ = 0; + for(VectorContactIterator i = contacts_.begin(); i != contacts_.end(); ++i) + { + if(i->isSet) + { + i->withRealSensor = false; + } + } + additionalForce_.setZero(); + additionalTorque_.setZero(); } } @@ -1327,91 +1501,726 @@ NoiseBase * KineticsObserver::getMeasurementNoise() const return measurementNoise_; } -Matrix KineticsObserver::computeAMatrix_() +Matrix KineticsObserver::computeAMatrix() +{ + // update of worldCentroidStateKinematics_ with the accelerations + estimateAccelerations(); + + const Vector & statePrediction = ekf_.updateStatePrediction(); + const Vector3 & predictedWorldCentroidStatePos = statePrediction.segment(posIndex()); + Orientation predictedWorldCentroidStateOri; + predictedWorldCentroidStateOri.fromVector4(statePrediction.segment(oriIndex())).toMatrix3(); + const Vector3 & predictedWorldCentroidStateLinVel = statePrediction.segment(linVelIndex()); + const Vector3 & predictedWorldCentroidStateAngVel = statePrediction.segment(angVelIndex()); + + Matrix A = Matrix::Zero(stateTangentSize_, stateTangentSize_); + + double dt2_2 = 0.5 * pow(dt_, 2); + Matrix3 dt2_2_Sp = dt2_2 * kine::skewSymmetric(worldCentroidStateKinematics_.position()); + + // Jacobians of the angular acceleration + Matrix3 I_inv = I_().inverse(); + // we can merge the two last lines not to create a new variable. We consider that the inertia matrix is given in the + // centroid's frame. + Matrix3 J_omegadot_ext_torque = I_inv; + + Matrix3 J_omegadot_omega = + I_inv + * (kine::skewSymmetric(I_() * worldCentroidStateKinematics_.angVel()) - Id_() + - kine::skewSymmetric(worldCentroidStateKinematics_.angVel()) * I_() + kine::skewSymmetric(sigma_())); + + // Jacobians of the linear acceleration + Matrix3 J_al_R = + -cst::gravityConstant + * (worldCentroidStateKinematics_.orientation.toMatrix3().transpose() * kine::skewSymmetric(Vector3(0, 0, 1))); + + // creation of the variables linked to the unmodeled wrench. Initialized after if required. + Matrix3 J_al_ext_force; + Matrix3 J_pl_ext_force; + Matrix3 J_pl_ext_torque; + Matrix3 J_R_ext_torque; + Matrix3 J_vl_ext_force; + Matrix3 J_omega_ext_torque; + + // Jacobians of the centroid's position + Matrix3 J_pl_pl = Matrix::Identity(sizePosTangent, sizePosTangent) + - dt_ * kine::skewSymmetric(worldCentroidStateKinematics_.angVel()) + + dt2_2 + * (kine::skewSymmetric2(worldCentroidStateKinematics_.angVel()) + - kine::skewSymmetric(worldCentroidStateKinematics_.angAcc())); + + A.block(posIndexTangent(), posIndexTangent()) = + J_pl_pl; // For optimization, we can merge the two last operations into one + Matrix3 J_pl_R = dt2_2 * J_al_R; + A.block(posIndexTangent(), oriIndexTangent()) = J_pl_R; + Matrix3 J_pl_vl = dt_ * Matrix::Identity(sizePosTangent, sizeLinVelTangent) + - 2.0 * dt2_2 * kine::skewSymmetric(worldCentroidStateKinematics_.angVel()); + A.block(posIndexTangent(), linVelIndexTangent()) = J_pl_vl; + Matrix3 J_pl_omega = kine::skewSymmetric(dt_ * worldCentroidStateKinematics_.position() + + 2.0 * dt2_2 * worldCentroidStateKinematics_.linVel()) + + dt2_2 + * (kine::skewSymmetric(worldCentroidStateKinematics_.position()) * J_omegadot_omega + + kine::skewSymmetric(kine::skewSymmetric(worldCentroidStateKinematics_.position()) + * worldCentroidStateKinematics_.angVel()) + - kine::skewSymmetric(worldCentroidStateKinematics_.angVel()) + * kine::skewSymmetric(worldCentroidStateKinematics_.position())); + A.block(posIndexTangent(), angVelIndexTangent()) = J_pl_omega; + + // Jacobians of the orientation + Vector delta = dt_ * worldCentroidStateKinematics_.angVel() + dt2_2 * worldCentroidStateKinematics_.angAcc(); + double sq_norm_delta = delta.squaredNorm(); + double norm_delta = delta.norm(); + double sin_delta_2 = sin(0.5 * norm_delta); + + Matrix3 J_R_delta; + if(norm_delta > cst::epsilonAngle) + { + J_R_delta.noalias() = 2.0 / norm_delta + * (((norm_delta - 2.0 * sin_delta_2) / (2.0 * sq_norm_delta)) + * (worldCentroidStateKinematics_.orientation.toMatrix3() * delta * delta.transpose()) + + sin_delta_2 + * (worldCentroidStateKinematics_.orientation.toMatrix3() + * kine::rotationVectorToRotationMatrix(0.5 * delta))); + } + else + { + J_R_delta.noalias() = + worldCentroidStateKinematics_.orientation.toMatrix3() * kine::rotationVectorToRotationMatrix(0.5 * delta); + } + + // the intermediate jacobian used to compute the ones with respect to the angular velocity and acceleration + Matrix3 J_R_omegadot = J_R_delta * dt2_2; // used in other Jacobians + + Matrix3 J_R_R = Matrix::Identity(sizeOriTangent, sizeOriTangent); + A.block(oriIndexTangent(), oriIndexTangent()) = J_R_R; + Matrix3 J_R_omega = + J_R_delta * (dt_ * Matrix::Identity(sizeAngVelTangent, sizeAngVelTangent) + dt2_2 * J_omegadot_omega); + A.block(oriIndexTangent(), angVelIndexTangent()) = J_R_omega; + + // Jacobians of the linear velocity + Matrix3 J_vl_R = dt_ * J_al_R; + A.block(linVelIndexTangent(), oriIndexTangent()) = J_vl_R; + Matrix3 J_vl_vl = Matrix::Identity(sizeLinVelTangent, sizeLinVelTangent) + - dt_ * kine::skewSymmetric(worldCentroidStateKinematics_.angVel()); + A.block(linVelIndexTangent(), linVelIndexTangent()) = J_vl_vl; + Matrix3 J_vl_omega = dt_ * kine::skewSymmetric(worldCentroidStateKinematics_.linVel()); + A.block(linVelIndexTangent(), angVelIndexTangent()) = J_vl_omega; + + // Jacobians of the angular velocity + Matrix3 J_omega_omega = Matrix3::Identity() + dt_ * J_omegadot_omega; + A.block(angVelIndexTangent(), angVelIndexTangent()) = J_omega_omega; + + // Jacobians of the gyrometer bias + if(withGyroBias_) + { + Matrix3 J_gyrobias_gyrobias = Matrix::Identity(sizeGyroBiasTangent, sizeGyroBiasTangent); + for(unsigned i = 0; i < imuSensors_.size(); ++i) + { + A.block(gyroBiasIndexTangent(i), gyroBiasIndexTangent(i)) = + J_gyrobias_gyrobias; + } + } + + // Jacobians of the unmodeled wrench external force + if(withUnmodeledWrench_) + { + // Jacobians of the unmodeled external force + Matrix3 J_ext_force_ext_force = Matrix::Identity(sizeForceTangent, sizeForceTangent); + A.block(unmodeledForceIndexTangent(), unmodeledForceIndexTangent()) = + J_ext_force_ext_force; + + // Jacobians with respect to the unmodeled external force + J_al_ext_force = Matrix::Identity(sizeLinAccTangent, sizeTorqueTangent) / mass_; + J_pl_ext_force = dt2_2 / mass_ * Matrix::Identity(sizePosTangent, sizeForceTangent); + A.block(posIndexTangent(), unmodeledForceIndexTangent()) = J_pl_ext_force; + J_vl_ext_force = dt_ * J_al_ext_force; + A.block(linVelIndexTangent(), unmodeledForceIndexTangent()) = J_vl_ext_force; + + // Jacobians of the unmodeled external torque + Matrix3 J_ext_torque_ext_torque = Matrix::Identity(sizeTorqueTangent, sizeTorqueTangent); + A.block(unmodeledTorqueIndexTangent(), unmodeledTorqueIndexTangent()) = + J_ext_torque_ext_torque; + + // Jacobians with respect to the unmodeled external torque + J_pl_ext_torque = dt2_2_Sp * J_omegadot_ext_torque; + A.block(posIndexTangent(), unmodeledTorqueIndexTangent()) = J_pl_ext_torque; + J_R_ext_torque = J_R_omegadot * J_omegadot_ext_torque; + A.block(oriIndexTangent(), unmodeledTorqueIndexTangent()) = J_R_ext_torque; + J_omega_ext_torque = dt_ * J_omegadot_ext_torque; + A.block(angVelIndexTangent(), unmodeledTorqueIndexTangent()) = + J_omega_ext_torque; + } + + // Jacobians with respect to contacts + Matrix3 J_poscontact_poscontact = + Matrix::Identity(sizePosTangent, sizePosTangent); // out of the loop as it is constant + // but then creates a useless variable if there is no contact + for(VectorContactConstIterator i = contacts_.begin(); i != contacts_.end(); ++i) + { + if(i->isSet) + { + Orientation predictedWorldContactRestOri; + predictedWorldContactRestOri.fromVector4(statePrediction.segment(contactOriIndex(i))).toMatrix3(); + Vector3 predictedWorldContactRestPosition = statePrediction.segment(contactPosIndex(i)); + + // Jacobian of the linar acceleration with respect to the contact force + Matrix3 J_linAcc_Fcis = (1 / mass_) * i->centroidContactKine.orientation.toMatrix3(); + // Jacobian of the angular acceleration with respect to the contact force + Matrix3 J_omegadot_Fcis = (I_inv * kine::skewSymmetric(i->centroidContactKine.position())) + * i->centroidContactKine.orientation.toMatrix3(); + // Jacobian of the angular acceleration with respect to the contact torque + Matrix3 J_omegadot_Tcis = I_inv * i->centroidContactKine.orientation.toMatrix3(); + + // Jacobian of the centroid's position with respect to the contact force + Matrix3 J_pl_contactForce = dt2_2_Sp * J_omegadot_Fcis + dt2_2 * J_linAcc_Fcis; + A.block(posIndexTangent(), contactForceIndexTangent(i)) = J_pl_contactForce; + Matrix3 J_pl_contactTorque = dt2_2_Sp * J_omegadot_Tcis; + A.block(posIndexTangent(), contactTorqueIndexTangent(i)) = J_pl_contactTorque; + // Jacobians of the orientation with respect to the contact force and torque + Matrix3 J_R_contactForce = J_R_omegadot * J_omegadot_Fcis; + A.block(oriIndexTangent(), contactForceIndexTangent(i)) = J_R_contactForce; + Matrix3 J_R_contactTorque = J_R_omegadot * J_omegadot_Tcis; + A.block(oriIndexTangent(), contactTorqueIndexTangent(i)) = J_R_contactTorque; + // Jacobian of the linear velocity with respect to the contact force + Matrix3 J_vl_contactForce = dt_ * J_linAcc_Fcis; + A.block(linVelIndexTangent(), contactForceIndexTangent(i)) = + J_vl_contactForce; + // Jacobian of the angular velocity with respect to the contact force and torque + Matrix3 J_omega_contactForce = dt_ * J_omegadot_Fcis; + A.block(angVelIndexTangent(), contactForceIndexTangent(i)) = + J_omega_contactForce; + Matrix3 J_omega_contactTorque = dt_ * J_omegadot_Tcis; + A.block(angVelIndexTangent(), contactTorqueIndexTangent(i)) = + J_omega_contactTorque; + + // Jacobian of the contact position and orientation with respect to themselves + A.block(contactPosIndexTangent(i), contactPosIndexTangent(i)) = + J_poscontact_poscontact; + Matrix3 J_contactOri_contactOri = J_poscontact_poscontact; + A.block(contactOriIndexTangent(i), contactOriIndexTangent(i)) = + J_contactOri_contactOri; + + Orientation contactWorldOri( + Matrix3(i->centroidContactKine.orientation.toMatrix3().transpose() + * predictedWorldCentroidStateOri.toMatrix3() + .transpose())); // better to compute it now as it is used in several expressions + // Jacobians of the contacts force + Matrix3 J_contactForce_pl_at_same_time = + -(contactWorldOri.toMatrix3() * i->linearStiffness * predictedWorldCentroidStateOri.toMatrix3()); + Vector3 sumVelContact = i->centroidContactKine.linVel() + + predictedWorldCentroidStateAngVel.cross(i->centroidContactKine.position()) + + predictedWorldCentroidStateLinVel; + + Matrix3 J_contactForce_R_at_same_time = + contactWorldOri.toMatrix3() + * (i->linearStiffness + * kine::skewSymmetric(predictedWorldCentroidStateOri.toMatrix3() + * (i->centroidContactKine.position() + predictedWorldCentroidStatePos)) + + i->linearDamping * kine::skewSymmetric(predictedWorldCentroidStateOri.toMatrix3() * sumVelContact) + - kine::skewSymmetric(i->linearStiffness + * (predictedWorldCentroidStateOri.toMatrix3() + * (i->centroidContactKine.position() + predictedWorldCentroidStatePos)) + + i->linearDamping * (predictedWorldCentroidStateOri.toMatrix3() * sumVelContact) + - i->linearStiffness * predictedWorldContactRestPosition)); + + Matrix3 J_contactForce_vl_at_same_time = + -(contactWorldOri.toMatrix3() * i->linearDamping * predictedWorldCentroidStateOri.toMatrix3()); + Matrix3 J_contactForce_omega_at_same_time = + contactWorldOri.toMatrix3() * i->linearDamping + * (predictedWorldCentroidStateOri.toMatrix3() * kine::skewSymmetric(i->centroidContactKine.position())); + Matrix3 J_contactForce_contactPosition_at_same_time = contactWorldOri.toMatrix3() * i->linearStiffness; + + A.block(contactForceIndexTangent(i), posIndexTangent()) = + J_contactForce_pl_at_same_time * J_pl_pl; + A.block(contactForceIndexTangent(i), oriIndexTangent()) = + J_contactForce_pl_at_same_time * J_pl_R + J_contactForce_R_at_same_time + + J_contactForce_vl_at_same_time * J_vl_R; + A.block(contactForceIndexTangent(i), linVelIndexTangent()) = + J_contactForce_pl_at_same_time * J_pl_vl + J_contactForce_vl_at_same_time * J_vl_vl; + A.block(contactForceIndexTangent(i), angVelIndexTangent()) = + J_contactForce_pl_at_same_time * J_pl_omega + J_contactForce_R_at_same_time * J_R_omega + + J_contactForce_vl_at_same_time * J_vl_omega + J_contactForce_omega_at_same_time * J_omega_omega; + A.block(contactForceIndexTangent(i), contactPosIndexTangent(i)) = + J_contactForce_contactPosition_at_same_time; + A.block(contactForceIndexTangent(i), contactForceIndexTangent(i)) = + J_contactForce_pl_at_same_time * J_pl_contactForce + J_contactForce_R_at_same_time * J_R_contactForce + + J_contactForce_vl_at_same_time * J_vl_contactForce + + J_contactForce_omega_at_same_time * J_omega_contactForce; + A.block(contactForceIndexTangent(i), contactTorqueIndexTangent(i)) = + J_contactForce_pl_at_same_time * J_pl_contactTorque + J_contactForce_R_at_same_time * J_R_contactTorque + + J_contactForce_omega_at_same_time * J_omega_contactTorque; + + // Jacobians of the contacts torque + Vector3 angVelSum = + predictedWorldCentroidStateOri * (i->centroidContactKine.angVel() + predictedWorldCentroidStateAngVel); + Vector3 ex = Vector3(1, 0, 0); + Vector3 ey = Vector3(0, 1, 0); + Vector3 ez = Vector3(0, 0, 1); + Orientation RRefContactToWorld = + predictedWorldCentroidStateOri * i->centroidContactKine.orientation * predictedWorldContactRestOri.inverse(); + Orientation RWorldToRefContact = RRefContactToWorld.inverse(); + + Matrix3 Vk = -ex * ez.transpose() + * (kine::skewSymmetric(RRefContactToWorld.toMatrix3() * ey) + + RWorldToRefContact.toMatrix3() * kine::skewSymmetric(ey)) + - ey * ex.transpose() + * (kine::skewSymmetric(RRefContactToWorld.toMatrix3() * ez) + + RWorldToRefContact.toMatrix3() * kine::skewSymmetric(ez)) + - ez * ey.transpose() + * (kine::skewSymmetric(RRefContactToWorld.toMatrix3() * ex) + + RWorldToRefContact.toMatrix3() * kine::skewSymmetric(ex)); + + Matrix3 Vk2 = -ex * ez.transpose() + * (kine::skewSymmetric(RWorldToRefContact.toMatrix3() * ey) + + RRefContactToWorld.toMatrix3() * kine::skewSymmetric(ey)) + - ey * ex.transpose() + * (kine::skewSymmetric(RWorldToRefContact.toMatrix3() * ez) + + RRefContactToWorld.toMatrix3() * kine::skewSymmetric(ez)) + - ez * ey.transpose() + * (kine::skewSymmetric(RWorldToRefContact.toMatrix3() * ex) + + RRefContactToWorld.toMatrix3() * kine::skewSymmetric(ex)); + + Matrix3 J_contactTorque_R_at_same_time = + -(contactWorldOri.toMatrix3() + * (kine::skewSymmetric(0.5 * i->angularStiffness + * (kine::skewSymmetricToRotationVector(RRefContactToWorld.toMatrix3() + - RWorldToRefContact.toMatrix3())) + + i->angularDamping * angVelSum) + + 0.5 * i->angularStiffness * Vk - i->angularDamping * kine::skewSymmetric(angVelSum))); + + Matrix3 J_contactTorque_omega_at_same_time = + -(contactWorldOri.toMatrix3() * i->angularDamping * predictedWorldCentroidStateOri.toMatrix3()); + + Matrix3 J_contactTorque_contactOri_at_same_time = 0.5 * (contactWorldOri.toMatrix3() * i->angularStiffness * Vk2); + + A.block(contactTorqueIndexTangent(i), oriIndexTangent()) = + J_contactTorque_R_at_same_time; + A.block(contactTorqueIndexTangent(i), angVelIndexTangent()) = + J_contactTorque_R_at_same_time * J_R_omega + J_contactTorque_omega_at_same_time * J_omega_omega; + A.block(contactTorqueIndexTangent(i), contactOriIndexTangent(i)) = + J_contactTorque_contactOri_at_same_time; + A.block(contactTorqueIndexTangent(i), contactForceIndexTangent(i)) = + J_contactTorque_R_at_same_time * J_R_contactForce + J_contactTorque_omega_at_same_time * J_omega_contactForce; + A.block(contactTorqueIndexTangent(i), contactTorqueIndexTangent(i)) = + J_contactTorque_R_at_same_time * J_R_contactTorque + + J_contactTorque_omega_at_same_time * J_omega_contactTorque; + + if(withUnmodeledWrench_) + { + A.block(contactForceIndexTangent(i), unmodeledForceIndexTangent()) = + J_contactForce_pl_at_same_time * J_pl_ext_force + J_contactForce_vl_at_same_time * J_vl_ext_force; + A.block(contactForceIndexTangent(i), unmodeledTorqueIndexTangent()) = + J_contactForce_pl_at_same_time * J_pl_ext_torque + J_contactForce_R_at_same_time * J_R_ext_torque + + J_contactForce_omega_at_same_time * J_omega_ext_torque; + A.block(contactTorqueIndexTangent(i), unmodeledTorqueIndexTangent()) = + J_contactTorque_R_at_same_time * J_R_ext_torque + J_contactTorque_omega_at_same_time * J_omega_ext_torque; + } + } + } + return A; +} + +Matrix KineticsObserver::computeCMatrix() +{ + const Vector & statePrediction = ekf_.updateStatePrediction(); + Orientation predictedWorldCentroidStateOri; + predictedWorldCentroidStateOri.fromVector4(statePrediction.segment(oriIndex())).toMatrix3(); + const Vector3 & predictedWorldCentroidStateAngVel = statePrediction.segment(angVelIndex()); + + Vector3 forceCentroid = additionalForce_; + Vector3 torqueCentroid = additionalTorque_; + + addUnmodeledAndContactWrench_(statePrediction, forceCentroid, torqueCentroid); + + LocalKinematics predictedWorldCentroidStateKinematics(statePrediction.segment(kineIndex()), + flagsStateKine); + + /// The accelerations are about to be computed so we set them to "initialized" + predictedWorldCentroidStateKinematics.linAcc.set(true); + predictedWorldCentroidStateKinematics.angAcc.set(true); + + Vector3 & linacc = predictedWorldCentroidStateKinematics.linAcc(); + Vector3 & angacc = predictedWorldCentroidStateKinematics.angAcc(); + + computeLocalAccelerations_(predictedWorldCentroidStateKinematics, forceCentroid, torqueCentroid, linacc, angacc); + + Matrix C = Matrix::Zero(measurementTangentSize_, stateTangentSize_); + + Matrix3 Iinv = I_().inverse(); + + // Jacobians of the gyrometer bias + for(VectorIMUConstIterator i = imuSensors_.begin(); i != imuSensors_.end(); ++i) + { + if(i->time == k_data_) + { + const IMU & imu = *i; + + Matrix3 oriCentroidToImu = imu.centroidImuKinematics.orientation.toMatrix3().transpose(); + + C.block(imu.measIndex, angVelIndexTangent()) = + -kine::skewSymmetric( + (oriCentroidToImu * predictedWorldCentroidStateAngVel).cross(imu.centroidImuKinematics.position())) + * oriCentroidToImu + - kine::skewSymmetric(oriCentroidToImu * predictedWorldCentroidStateAngVel) + * kine::skewSymmetric(imu.centroidImuKinematics.position()) * oriCentroidToImu + - kine::skewSymmetric(imu.centroidImuKinematics.position()) * oriCentroidToImu + * (Iinv + * (kine::skewSymmetric(I_() * predictedWorldCentroidStateAngVel) - Id_() + - kine::skewSymmetric(predictedWorldCentroidStateAngVel) * I_() + kine::skewSymmetric(sigma_()))) + - 2 * kine::skewSymmetric(imu.centroidImuKinematics.linVel()) * oriCentroidToImu; + + C.block(imu.measIndex, unmodeledForceIndexTangent()) = + 1.0 / mass_ * oriCentroidToImu; + + C.block(imu.measIndex, unmodeledTorqueIndexTangent()) = + -kine::skewSymmetric(imu.centroidImuKinematics.position()) * oriCentroidToImu * Iinv; + + for(VectorContactConstIterator i = contacts_.begin(); i != contacts_.end(); ++i) + { + if(i->isSet) + { + C.block(imu.measIndex, contactForceIndexTangent(i)) = + oriCentroidToImu * 1.0 / mass_ * i->centroidContactKine.orientation.toMatrix3() + - kine::skewSymmetric(imu.centroidImuKinematics.position()) * oriCentroidToImu * Iinv + * kine::skewSymmetric(i->centroidContactKine.position()) + * i->centroidContactKine.orientation.toMatrix3(); + + C.block(imu.measIndex, contactTorqueIndexTangent(i)) = + -kine::skewSymmetric(imu.centroidImuKinematics.position()) * oriCentroidToImu * Iinv + * i->centroidContactKine.orientation.toMatrix3(); + } + } + + /// gyrometer + C.block(imu.measIndex + sizeAcceleroSignal, angVelIndexTangent()) = + oriCentroidToImu; + + if(withGyroBias_) + { + C.block(imu.measIndex + sizeAcceleroSignal, gyroBiasIndexTangent(imu.num)) = + Matrix3::Identity(); + } + } + } + + for(VectorContactConstIterator i = contacts_.begin(); i != contacts_.end(); ++i) + { + if(i->withRealSensor) + { + C.block(i->measIndex, contactForceIndexTangent(i)) = Matrix3::Identity(); + C.block(i->measIndex + sizeForceTangent, contactTorqueIndexTangent(i)) = + Matrix3::Identity(); + } + } + + if(absPoseSensor_.time == k_data_) + { + C.block(absPoseSensor_.measIndex, posIndexTangent()) = + predictedWorldCentroidStateOri.toMatrix3(); + C.block(absPoseSensor_.measIndex + sizePosTangent, oriIndexTangent()) = + Matrix3::Identity(); + } + + return C; +} + +void KineticsObserver::convertUserToCentroidFrame_(const Kinematics & userKine, + Kinematics & centroidKine, + TimeIndex k_data) { - return ekf_.getAMatrixFD(stateVectorDx_); + BOOST_ASSERT((com_.getTime() == k_data && com_.getTime() == comd_.getTime() && com_.getTime() == comdd_.getTime()) + && "The Center of Mass must be actualized before the conversion"); + centroidKine.position = userKine.position() - com_(); + if(userKine.linVel.isSet()) + { + centroidKine.linVel = userKine.linVel() - comd_(); + } + if(userKine.linAcc.isSet()) + { + centroidKine.linAcc = userKine.linAcc() - comdd_(); + } + if(userKine.orientation.isSet()) + { + centroidKine.orientation = userKine.orientation; + } + if(userKine.angVel.isSet()) + { + centroidKine.angVel = userKine.angVel(); + } + if(userKine.angAcc.isSet()) + { + centroidKine.angAcc = userKine.angAcc(); + } } -Matrix KineticsObserver::computeCMatrix_() +KineticsObserver::Kinematics KineticsObserver::convertUserToCentroidFrame_(const Kinematics & userKine, + TimeIndex k_data) { - return ekf_.getCMatrixFD(stateVectorDx_); + Kinematics centroidKine; + BOOST_ASSERT((com_.getTime() == k_data && com_.getTime() == comd_.getTime() && com_.getTime() == comdd_.getTime()) + && "The Center of Mass must be actualized before the conversion"); + centroidKine.position = userKine.position() - com_(); + if(userKine.linVel.isSet()) + { + centroidKine.linVel = userKine.linVel() - comd_(); + } + if(userKine.linAcc.isSet()) + { + centroidKine.linAcc = userKine.linAcc() - comdd_(); + } + if(userKine.orientation.isSet()) + { + centroidKine.orientation = userKine.orientation; + } + if(userKine.angVel.isSet()) + { + centroidKine.angVel = userKine.angVel(); + } + if(userKine.angAcc.isSet()) + { + centroidKine.angAcc = userKine.angAcc(); + } + return centroidKine; } -void KineticsObserver::updateKine_() +void KineticsObserver::updateLocalKineAndContacts_() { - stateKinematics_.fromVector(stateVector_.segment(kineIndex()), flagsStateKine); + worldCentroidStateKinematics_.fromVector(worldCentroidStateVector_.segment(kineIndex()), + flagsStateKine); + for(VectorContactIterator i = contacts_.begin(); i != contacts_.end(); ++i) + { + if(i->isSet) + { + i->worldRestPose.fromVector(worldCentroidStateVector_.segment(contactPosIndex(i)), flagsContactKine); + } + } } -void KineticsObserver::addUnmodeledAndContactWrench_(const Vector & stateVector, Vector3 & force, Vector3 & torque) +void KineticsObserver::updateGlobalKine_() { - force += stateVector.segment(unmodeledForceIndex()); - torque += stateVector.segment(unmodeledTorqueIndex()); + worldCentroidKinematics_ = worldCentroidStateKinematics_; +} + +void KineticsObserver::addUnmodeledAndContactWrench_(const Vector & worldCentroidStateVector, + Vector3 & force, + Vector3 & torque) +{ + force += worldCentroidStateVector.segment(unmodeledWrenchIndex()); + + torque += worldCentroidStateVector.segment(unmodeledTorqueIndex()); for(VectorContactIterator i = contacts_.begin(); i != contacts_.end(); ++i) { if(i->isSet) { - Kinematics & localKinei = i->localKine; - Vector3 localForcei = localKinei.orientation * stateVector.segment(contactForceIndex(i)); - force += localForcei; - torque += localKinei.orientation * stateVector.segment(contactTorqueIndex(i)) - + localKinei.position().cross(localForcei); + Kinematics & centroidContactKinei = i->centroidContactKine; + Vector3 centroidContactForcei = + centroidContactKinei.orientation * worldCentroidStateVector.segment(contactForceIndex(i)); + + force += centroidContactForcei; + + torque += centroidContactKinei.orientation * worldCentroidStateVector.segment(contactTorqueIndex(i)) + + centroidContactKinei.position().cross(centroidContactForcei); } } } -void KineticsObserver::computeAccelerations_(Kinematics & stateKine, - const Vector3 & totalForceLocal, - const Vector3 & totalMomentLocal, - Vector3 & linAcc, - Vector3 & angAcc) +void KineticsObserver::addUnmodeledWrench_(const Vector & worldCentroidStateVector, Vector3 & force, Vector3 & torque) +{ + force += worldCentroidStateVector.segment(unmodeledWrenchIndex()); + torque += worldCentroidStateVector.segment(unmodeledTorqueIndex()); +} + +void KineticsObserver::addContactWrench_(const Kinematics & centroidContactKine, + const Vector3 & centroidContactForce, + const Vector3 & centroidContactTorque, + Vector3 & totalCentroidForce, + Vector3 & totalCentroidTorque) { - Matrix3 Rt = stateKine.orientation.toMatrix3().transpose(); - Vector3 Rtw = Rt * stateKine.angVel(); - Vector3 corioCentri = 2 * Rtw.cross(comd_() + Rtw.cross(com_())); + totalCentroidForce += centroidContactForce; + totalCentroidTorque += centroidContactTorque + centroidContactKine.position().cross(centroidContactForce); +} - angAcc = stateKine.orientation - * ((I_() + mass_ * kine::skewSymmetric2(com_())).inverse() - * (totalMomentLocal - Id_() * Rtw - sigmad_() - Rtw.cross(I_() * Rtw + sigma_()) - - com_().cross(totalForceLocal - mass_ * (comdd_() + corioCentri)))); +void KineticsObserver::computeLocalAccelerations_(LocalKinematics & worldCentroidStateKinematics, + const Vector3 & totalCentroidForce, + const Vector3 & totalCentroidTorque, + Vector3 & linAcc, + Vector3 & angAcc) +{ + angAcc = I_().inverse() + * (totalCentroidTorque - Id_() * worldCentroidStateKinematics.angVel() - sigmad_() + - worldCentroidStateKinematics.angVel().cross(I_() * worldCentroidStateKinematics.angVel() + sigma_())); - linAcc = stateKine.orientation * ((totalForceLocal / mass_) - comdd_() - corioCentri + com_().cross(Rt * angAcc)) - - cst::gravity; + linAcc = + (totalCentroidForce / mass_) - worldCentroidStateKinematics.orientation.toMatrix3().transpose() * cst::gravity; } -void KineticsObserver::computeContactForces_(VectorContactIterator i, - Kinematics & stateKine, - Kinematics & contactPose, - Vector3 & force, - Vector3 torque) +void KineticsObserver::computeLocalAccelerations(const Vector & x, Vector & acceleration) +{ + // initialization of the total force at the centroid with the input additional forces. + Vector3 totalCentroidForce = additionalForce_; + Vector3 totalCentroidTorque = additionalTorque_; + + // adding the previously estimated contact and unmodeled forces to obtain the total force at the centroid + addUnmodeledAndContactWrench_(x, totalCentroidForce, totalCentroidTorque); + LocalKinematics worldCentroidStateKinematics(x, flagsStateKine); + + acceleration.segment<3>(0) = + (totalCentroidForce / mass_) - worldCentroidStateKinematics.orientation.toMatrix3().transpose() * cst::gravity; + + acceleration.segment<3>(3) = + I_().inverse() + * (totalCentroidTorque - Id_() * worldCentroidStateKinematics.angVel() - sigmad_() + - worldCentroidStateKinematics.angVel().cross(I_() * worldCentroidStateKinematics.angVel() + sigma_())); +} + +void KineticsObserver::computeRecursiveGlobalAccelerations_( + Kinematics & predictedWorldCentroidKinematics) // used in the Runge-Kutta integration +{ + BOOST_ASSERT(false && "this function must be checked"); + Vector3 predictedVEContactForces = Vector3::Zero(); + Vector3 predictedVEContactTorques = Vector3::Zero(); + + LocalKinematics predictedLocalWorldCentroidKinematics(predictedWorldCentroidKinematics); + + computeContactForces_(predictedLocalWorldCentroidKinematics, predictedVEContactForces, predictedVEContactTorques); + + // increment of force occuring during the current integration step of the rungeKutta + Vector3 diffForces = predictedVEContactForces - initialVEContactForces_; + Vector3 diffTorques = predictedVEContactTorques - initialVEContactTorques_; + + Vector3 & currentStepContactForces = diffForces; + Vector3 & currentStepContactTorques = diffTorques; + + // Corresponds to the increment contactForce + currentStepContactForces += initTotalCentroidForce_; + currentStepContactTorques += initTotalCentroidTorque_; + + predictedLocalWorldCentroidKinematics.linAcc = + predictedLocalWorldCentroidKinematics.orientation * (currentStepContactForces / mass_) - cst::gravity; + + predictedLocalWorldCentroidKinematics.angAcc = + predictedLocalWorldCentroidKinematics.orientation.toMatrix3() * I_().inverse() + * (currentStepContactTorques + - Id_() * predictedLocalWorldCentroidKinematics.orientation.toMatrix3().transpose() + * predictedLocalWorldCentroidKinematics.angVel() + - sigmad_() + - (predictedLocalWorldCentroidKinematics.orientation.toMatrix3().transpose() + * predictedLocalWorldCentroidKinematics.angVel()) + .cross(I_() * predictedLocalWorldCentroidKinematics.orientation.toMatrix3().transpose() + * predictedLocalWorldCentroidKinematics.angVel() + + sigma_())); +} + +void KineticsObserver::computeRecursiveLocalAccelerations_( + LocalKinematics & predictedWorldCentroidKinematics) // used in the Runge-Kutta integration +{ + Kinematics globWorldCentroidStateKinematics(predictedWorldCentroidKinematics); + + Vector3 predictedVEContactForces = Vector3::Zero(); + Vector3 predictedVEContactTorques = Vector3::Zero(); + + computeContactForces_(predictedWorldCentroidKinematics, predictedVEContactForces, predictedVEContactTorques); + + // computing the increment of force occuring during the current integration step of the rungeKutta + Vector3 diffForces = predictedVEContactForces - initialVEContactForces_; + Vector3 diffTorques = predictedVEContactTorques - initialVEContactTorques_; + + Vector3 & currentStepContactForces = diffForces; + Vector3 & currentStepContactTorques = diffTorques; + + // Corresponds to the increment contactForce + currentStepContactForces += initTotalCentroidForce_; + currentStepContactTorques += initTotalCentroidTorque_; + + predictedWorldCentroidKinematics.linAcc = + (currentStepContactForces / mass_) + - predictedWorldCentroidKinematics.orientation.toMatrix3().transpose() * cst::gravity; + predictedWorldCentroidKinematics.angAcc = + I_().inverse() + * (currentStepContactTorques - Id_() * predictedWorldCentroidKinematics.angVel() - sigmad_() + - predictedWorldCentroidKinematics.angVel().cross(I_() * predictedWorldCentroidKinematics.angVel() + + sigma_())); +} + +void KineticsObserver::computeContactForce_(VectorContactIterator i, + LocalKinematics & worldCentroidStateKinematics, + Kinematics & worldRestContactPose, + Vector3 & contactForce, + Vector3 & contactTorque) { Contact & contact = *i; - Kinematics & localKine = contact.localKine; + // the kinematics of the contact in the centroid's frame, expressed in the centroid's frame + Kinematics & centroidContactKine = contact.centroidContactKine; + // the kinematics of the contact in the world frame, expressed in the contact's frame + Kinematics & worldContactPose = contact.temp.worldContactPose; + worldContactPose.setToProductNoAlias(Kinematics(worldCentroidStateKinematics), centroidContactKine); - Kinematics globalKine(stateKine, localKine); /// product of kinematics + contactForce = worldCentroidStateKinematics.orientation.toMatrix3().transpose() + * (contact.linearStiffness * (worldRestContactPose.position() - worldContactPose.position()) + - contact.linearDamping * worldContactPose.linVel()); - Matrix3 globKineOriInverse = globalKine.orientation.toMatrix3().transpose(); + contactTorque = + worldCentroidStateKinematics.orientation.toMatrix3().transpose() + * (-0.5 * contact.angularStiffness + * (worldContactPose.orientation.toQuaternion() * worldRestContactPose.orientation.toQuaternion().inverse()) + .vec() + - contact.angularDamping * worldContactPose.angVel()); +} + +void KineticsObserver::computeContactForces_(LocalKinematics & worldCentroidStateKinematics, + Vector3 & contactForce, + Vector3 & contactTorque) +{ + BOOST_ASSERT(contactForce.isZero() && "The contact forces must be initialized with a zero vector"); + BOOST_ASSERT(contactTorque.isZero() && "The contact forces must be initialized with a zero vector"); - force = globKineOriInverse - * (contact.linearStiffness * (contactPose.position() - globalKine.position()) - - contact.linearDamping * globalKine.linVel()); - torque = globKineOriInverse - * (-0.5 * contact.angularStiffness - * (globalKine.orientation.toQuaternion() * contactPose.orientation.toQuaternion().inverse()).vec() - - contact.angularDamping * globalKine.angVel()); + for(VectorContactIterator i = contacts_.begin(); i != contacts_.end(); ++i) + { + if(i->isSet) + { + Contact & contact = *i; + + // the kinematics of the contact in the centroid's frame, expressed in the centroid's frame + Kinematics & centroidContactKine = contact.centroidContactKine; + // the kinematics of the contact in the world frame, expressed in the contact's frame + Kinematics & worldContactPose = contact.temp.worldContactPose; + // the rest kinematics of the contact in the world frame, expressed in the contact's frame + Kinematics & worldRestContactPose = contact.worldRestPose; + + worldContactPose.setToProductNoAlias(Kinematics(worldCentroidStateKinematics), centroidContactKine); + + Vector3 centroidContactForce = + worldCentroidStateKinematics.orientation.toMatrix3().transpose() + * (contact.linearStiffness * (worldRestContactPose.position() - worldContactPose.position()) + - contact.linearDamping * worldContactPose.linVel()); + contactForce += centroidContactForce; + + Vector3 centroidContactTorque = worldCentroidStateKinematics.orientation.toMatrix3().transpose() + * (-0.5 * contact.angularStiffness + * (worldContactPose.orientation.toQuaternion() + * worldRestContactPose.orientation.toQuaternion().inverse()) + .vec() + - contact.angularDamping * worldContactPose.angVel()); + + contactTorque += centroidContactTorque + centroidContactKine.position().cross(centroidContactForce); + } + } } -void KineticsObserver::stateSum(const Vector & stateVector, const Vector & tangentVector, Vector & sum) +void KineticsObserver::stateSum(const Vector & worldCentroidStateVector, const Vector & tangentVector, Vector & sum) { Orientation & o = opt_.ori; - sum = stateVector; + sum = worldCentroidStateVector; /// use the exponential map integration to perform the sum of the states sum.segment(posIndex()) += tangentVector.segment(posIndexTangent()); - o.fromVector4(stateVector.segment(oriIndex())); - o.integrate(tangentVector.segment(oriIndexTangent())); + o.fromVector4(worldCentroidStateVector.segment(oriIndex())); + o.integrate(tangentVector.segment( + oriIndexTangent())); // we don't use integrateRightOrientation() + // as it is used only for the computation of the dynamical evolution of the orientation by + // integration sum.segment(oriIndex()) = o.toVector4(); - /// sum.segment(linVelIndex()) += tangentVector.segment(linVelIndexTangent()); if(withGyroBias_) @@ -1425,13 +2234,12 @@ void KineticsObserver::stateSum(const Vector & stateVector, const Vector & tange { sum.segment(unmodeledWrenchIndex()) += tangentVector.segment(unmodeledWrenchIndexTangent()); } - for(VectorContactConstIterator i = contacts_.begin(); i != contacts_.end(); ++i) { if(i->isSet) { sum.segment(contactPosIndex(i)) += tangentVector.segment(contactPosIndexTangent(i)); - o.fromVector4(stateVector.segment(contactOriIndex(i))); + o.fromVector4(worldCentroidStateVector.segment(contactOriIndex(i))); o.integrate(tangentVector.segment(contactOriIndexTangent(i))); sum.segment(contactOriIndex(i)) = o.toVector4(); sum.segment(contactWrenchIndex(i)) += tangentVector.segment(contactWrenchIndexTangent(i)); @@ -1439,32 +2247,36 @@ void KineticsObserver::stateSum(const Vector & stateVector, const Vector & tange } } -void KineticsObserver::stateDifference(const Vector & stateVector1, const Vector & stateVector2, Vector & difference) +void KineticsObserver::stateDifference(const Vector & worldCentroidStateVector1, + const Vector & worldCentroidStateVector2, + Vector & difference) { Orientation & o1 = opt_.ori1; Orientation & o2 = opt_.ori2; difference.resize(stateTangentSize_); + difference.setZero(); difference.segment(posIndexTangent()).noalias() = - stateVector1.segment(posIndex()) - stateVector2.segment(posIndex()); - o1.fromVector4(stateVector1.segment(oriIndex())); - o2.fromVector4(stateVector2.segment(oriIndex())); + worldCentroidStateVector1.segment(posIndex()) - worldCentroidStateVector2.segment(posIndex()); + o1.fromVector4(worldCentroidStateVector1.segment(oriIndex())); + o2.fromVector4(worldCentroidStateVector2.segment(oriIndex())); difference.segment(oriIndexTangent()) = o2.differentiate(o1); difference.segment(linVelIndexTangent()).noalias() = - stateVector1.segment(linVelIndex()) - - stateVector2.segment(linVelIndex()); + worldCentroidStateVector1.segment(linVelIndex()) + - worldCentroidStateVector2.segment(linVelIndex()); if(withGyroBias_) { for(unsigned i = 0; i < imuSensors_.size(); ++i) { difference.segment(gyroBiasIndexTangent(i)).noalias() = - stateVector1.segment(gyroBiasIndex(i)) - stateVector2.segment(gyroBiasIndex(i)); + worldCentroidStateVector1.segment(gyroBiasIndex(i)) + - worldCentroidStateVector2.segment(gyroBiasIndex(i)); } } if(withUnmodeledWrench_) { difference.segment(unmodeledForceIndexTangent()).noalias() = - stateVector1.segment(unmodeledWrenchIndex()) - - stateVector2.segment(unmodeledWrenchIndex()); + worldCentroidStateVector1.segment(unmodeledWrenchIndex()) + - worldCentroidStateVector2.segment(unmodeledWrenchIndex()); } for(VectorContactConstIterator i = contacts_.begin(); i != contacts_.end(); ++i) @@ -1472,13 +2284,14 @@ void KineticsObserver::stateDifference(const Vector & stateVector1, const Vector if(i->isSet) { difference.segment(contactPosIndexTangent(i)).noalias() = - stateVector1.segment(contactPosIndex(i)) - stateVector2.segment(contactPosIndex(i)); - o1.fromVector4(stateVector1.segment(contactOriIndex(i))); - o2.fromVector4(stateVector2.segment(contactOriIndex(i))); + worldCentroidStateVector1.segment(contactPosIndex(i)) + - worldCentroidStateVector2.segment(contactPosIndex(i)); + o1.fromVector4(worldCentroidStateVector1.segment(contactOriIndex(i))); + o2.fromVector4(worldCentroidStateVector2.segment(contactOriIndex(i))); difference.segment(contactOriIndexTangent(i)) = o2.differentiate(o1); difference.segment(contactWrenchIndexTangent(i)).noalias() = - stateVector1.segment(contactWrenchIndex(i)) - - stateVector2.segment(contactWrenchIndex(i)); + worldCentroidStateVector1.segment(contactWrenchIndex(i)) + - worldCentroidStateVector2.segment(contactWrenchIndex(i)); } } } @@ -1514,54 +2327,80 @@ void KineticsObserver::measurementDifference(const Vector & measureVector1, Vector KineticsObserver::stateDynamics(const Vector & xInput, const Vector & /*unused*/, TimeIndex) { Vector x = xInput; - Vector3 forceLocal = additionalForce_; - Vector3 torqueLocal = additionalTorque_; + // initialization of the total force at the centroid with the input additional forces. + initTotalCentroidForce_ = additionalForce_; + initTotalCentroidTorque_ = additionalTorque_; - addUnmodeledAndContactWrench_(x, forceLocal, torqueLocal); + // adding the previously estimated contact and unmodeled forces to obtain the total force at the centroid + addUnmodeledAndContactWrench_(x, initTotalCentroidForce_, initTotalCentroidTorque_); - Kinematics stateKine(x.segment(kineIndex()), flagsStateKine); + LocalKinematics worldCentroidStateKinematics(x.segment(kineIndex()), flagsStateKine); - /// The accelerations are about to be computed so we set them to "initialized" - stateKine.linAcc.set(true); - stateKine.angAcc.set(true); + /// The accelerations are about to be computed so we initialize them + worldCentroidStateKinematics.linAcc = Vector3::Zero(); + worldCentroidStateKinematics.angAcc = Vector3::Zero(); - Vector3 & linacc = stateKine.linAcc(); /// reference (Vector3&) - Vector3 & angacc = stateKine.angAcc(); /// reference + Vector3 & linacc = worldCentroidStateKinematics.linAcc(); /// reference (Vector3&) + Vector3 & angacc = worldCentroidStateKinematics.angAcc(); /// reference - computeAccelerations_(stateKine, forceLocal, torqueLocal, linacc, angacc); + computeLocalAccelerations_(worldCentroidStateKinematics, initTotalCentroidForce_, initTotalCentroidTorque_, linacc, + angacc); - stateKine.integrate(dt_); + if(withRungeKutta_) + { + initialVEContactForces_ = Vector3::Zero(); + initialVEContactTorques_ = Vector3::Zero(); + computeContactForces_(worldCentroidStateKinematics, initialVEContactForces_, initialVEContactTorques_); - x.segment(kineIndex()) = stateKine.toVector(flagsStateKine); + worldCentroidStateKinematics.integrateRungeKutta4(dt_, *this); + } + else + { + worldCentroidStateKinematics.integrate(dt_); + } + + x.segment(kineIndex()) = worldCentroidStateKinematics.toVector(flagsStateKine); + + Kinematics globWorldCentroidStateKinematics = Kinematics(worldCentroidStateKinematics); + + if(!withGyroBias_) + { + for(VectorIMUIterator i = imuSensors_.begin(), ie = imuSensors_.end(); i != ie; ++i) + { + x.segment(gyroBiasIndex(i->num)).setZero(); + } + } + if(!withUnmodeledWrench_) + { + x.segment(unmodeledForceIndex()).setZero(); + } for(VectorContactIterator i = contacts_.begin(); i != contacts_.end(); ++i) { if(i->isSet) { - Kinematics & localKine = i->localKine; + Kinematics & centroidContactKine = i->centroidContactKine; + Kinematics worldContactRefPose; // not using the variable belonging to Contact as this variable must change only + // at the end of the update + worldContactRefPose.fromVector(x.segment(contactPosIndex(i)), flagsPoseKine); Matrix3 & Kpt = i->linearStiffness; Matrix3 & Kdt = i->linearDamping; Matrix3 & Kpr = i->angularStiffness; Matrix3 & Kdr = i->angularDamping; - /// the posiiton of the contact in the global frame - Kinematics globalKine; - - globalKine.setToProductNoAlias(stateKine, localKine); - - /// The error between the current kinematics and the rest kinematics - /// of the flexibility - Kinematics errorKine; - errorKine.setToProductNoAlias(globalKine, localKine.getInverse()); + // the pose of the contact in the world frame, expressed in the contact's frame + Kinematics & worldContactPose = i->temp.worldContactPose; + worldContactPose.setToProductNoAlias(globWorldCentroidStateKinematics, centroidContactKine); - /// Inverse of the orientation of the foot in the global frame - Orientation Rcit(globalKine.orientation.inverse()); + x.segment(contactForceIndex(i)) = + -(worldContactPose.orientation.toMatrix3().transpose() + * (Kpt * (worldContactPose.position() - worldContactRefPose.position()) + Kdt * worldContactPose.linVel())); - x.segment(contactForceIndex(i)) = -(Rcit * (Kpt * errorKine.position() + Kdt * errorKine.linVel())); - - x.segment(contactTorqueIndex(i)) = -( - Rcit * (Kpr * kine::vectorComponent(errorKine.orientation.toQuaternion()) * 0.5 + Kdr * errorKine.angVel())); + Matrix R = worldContactPose.orientation.toMatrix3() * worldContactRefPose.orientation.toMatrix3().transpose(); + x.segment(contactTorqueIndex(i)) = + -worldContactPose.orientation.toMatrix3().transpose() + * (0.5 * Kpr * kine::skewSymmetricToRotationVector(R - R.transpose()) + Kdr * worldContactPose.angVel()); } } @@ -1573,42 +2412,52 @@ Vector KineticsObserver::stateDynamics(const Vector & xInput, const Vector & /*u return x; } -Vector KineticsObserver::measureDynamics(const Vector & x, const Vector & /*unused*/, TimeIndex k) +Vector KineticsObserver::measureDynamics(const Vector & x_bar, const Vector & /*unused*/, TimeIndex k) { Vector y(getMeasurementSize()); - Vector3 forceLocal = additionalForce_; - Vector3 torqueLocal = additionalTorque_; + Vector3 forceCentroid = additionalForce_; + Vector3 torqueCentroid = additionalTorque_; - addUnmodeledAndContactWrench_(x, forceLocal, torqueLocal); + addUnmodeledAndContactWrench_(x_bar, forceCentroid, torqueCentroid); - Kinematics stateKine(x.segment(kineIndex()), flagsStateKine); + LocalKinematics worldCentroidStateKinematics(x_bar.segment(kineIndex()), flagsStateKine); - /// The accelerations are about to be computed so we set them to "initialized" - stateKine.linAcc.set(true); - stateKine.angAcc.set(true); + /// The accelerations are about to be computed so we initialize them - Vector3 & linacc = (Vector3 &)(stateKine.linAcc); - Vector3 & angacc = (Vector3 &)(stateKine.angAcc); + worldCentroidStateKinematics.linAcc = Vector3::Zero(); + worldCentroidStateKinematics.angAcc = Vector3::Zero(); - computeAccelerations_(stateKine, forceLocal, torqueLocal, linacc, angacc); + Vector3 & linacc = worldCentroidStateKinematics.linAcc(); + Vector3 & angacc = worldCentroidStateKinematics.angAcc(); - Kinematics & globalKine = opt_.kine; + computeLocalAccelerations_(worldCentroidStateKinematics, forceCentroid, torqueCentroid, linacc, angacc); for(VectorIMUConstIterator i = imuSensors_.begin(); i != imuSensors_.end(); ++i) { if(i->time == k_data_) { const IMU & imu = *i; - globalKine.setToProductNoAlias(stateKine, imu.kinematics); - const Matrix3 & globalOri = globalKine.orientation.toMatrix3(); + // the kinematics of the IMU in the world frame, expressed in the IMU's frame + LocalKinematics worldImuKinematics; + worldImuKinematics.setToProductNoAlias(worldCentroidStateKinematics, imu.centroidImuKinematics); + + const Matrix3 & worldImuOri = worldImuKinematics.orientation.toMatrix3(); /// accelerometer y.segment(imu.measIndex).noalias() = - globalOri.transpose() * (globalKine.linAcc() + cst::gravity); + worldImuKinematics.linAcc() + worldImuOri.transpose() * cst::gravity; + /// gyrometer - y.segment(imu.measIndex + sizeAcceleroSignal).noalias() = - globalOri.transpose() * globalKine.angVel(); + if(withGyroBias_) + { + y.segment(imu.measIndex + sizeAcceleroSignal).noalias() = + worldImuKinematics.angVel() + x_bar.segment(gyroBiasIndex(imu.num)); + } + else + { + y.segment(imu.measIndex + sizeAcceleroSignal).noalias() = worldImuKinematics.angVel(); + } } } @@ -1616,21 +2465,227 @@ Vector KineticsObserver::measureDynamics(const Vector & x, const Vector & /*unus { if(i->isSet && i->time == k_data_ && i->withRealSensor) { - y.segment(i->measIndex) = x.segment(contactWrenchIndex(i)); + y.segment(i->measIndex) = x_bar.segment(contactWrenchIndex(i)); } } if(absPoseSensor_.time == k) { - y.segment(absPoseSensor_.measIndex) = stateKine.toVector(flagsPoseKine); + y.segment(absPoseSensor_.measIndex) = + worldCentroidStateKinematics.orientation.toMatrix3() * worldCentroidStateKinematics.toVector(flagsPosKine); + y.segment(absPoseSensor_.measIndex + sizePos) = worldCentroidStateKinematics.orientation.toVector4(); } if(measurementNoise_ != 0x0) { measurementNoise_->getNoisy(y); } - return y; } +void KineticsObserver::setInitWorldCentroidStateVector(const Vector & initStateVector) +{ + worldCentroidStateVector_.setZero(); + setStateVector(initStateVector, false); +} + +void KineticsObserver::setAllCovariances(const Matrix3 & statePositionInitCovariance, + const Matrix3 & stateOriInitCovariance, + const Matrix3 & stateLinVelInitCovariance, + const Matrix3 & stateAngVelInitCovariance, + const Matrix3 & gyroBiasInitCovariance, + const Matrix6 & unmodeledWrenchInitCovariance, + const Matrix12 & contactInitCovariance, + const Matrix3 & statePositionProcessCovariance, + const Matrix3 & stateOriProcessCovariance, + const Matrix3 & stateLinVelProcessCovariance, + const Matrix3 & stateAngVelProcessCovariance, + const Matrix3 & gyroBiasProcessCovariance, + const Matrix6 & unmodeledWrenchProcessCovariance, + const Matrix12 & contactProcessCovariance, + const Matrix3 & positionSensorCovariance, + const Matrix3 & orientationSensorCoVariance, + const Matrix3 & acceleroSensorCovariance, + const Matrix3 & gyroSensorCovariance, + const Matrix6 & contactSensorCovariance) +{ + statePosInitCovMat_ = statePositionInitCovariance; + stateOriInitCovMat_ = stateOriInitCovariance; + stateLinVelInitCovMat_ = stateLinVelInitCovariance; + stateAngVelInitCovMat_ = stateAngVelInitCovariance; + gyroBiasInitCovMat_ = gyroBiasInitCovariance; + unmodeledWrenchInitCovMat_ = unmodeledWrenchInitCovariance; + contactInitCovMatDefault_ = contactInitCovariance; + + statePosProcessCovMat_ = statePositionProcessCovariance; + stateOriProcessCovMat_ = stateOriProcessCovariance; + stateLinVelProcessCovMat_ = stateLinVelProcessCovariance; + stateAngVelProcessCovMat_ = stateAngVelProcessCovariance; + gyroBiasProcessCovMat_ = gyroBiasProcessCovariance; + unmodeledWrenchProcessCovMat_ = unmodeledWrenchProcessCovariance; + contactProcessCovMatDefault_ = contactProcessCovariance; + + absPoseSensorCovMatDefault_ = + blockMat6(positionSensorCovariance, Matrix3::Zero(), Matrix3::Zero(), orientationSensorCoVariance); + acceleroCovMatDefault_ = acceleroSensorCovariance; + gyroCovMatDefault_ = gyroSensorCovariance; + contactWrenchSensorCovMatDefault_ = contactSensorCovariance; + + stateKinematicsInitCovMat_.block(posIndexTangent(), posIndexTangent()) = statePosInitCovMat_; + stateKinematicsInitCovMat_.block(oriIndexTangent(), oriIndexTangent()) = + stateOriInitCovMat_; + stateKinematicsInitCovMat_.block(linVelIndexTangent(), linVelIndexTangent()) = + stateLinVelInitCovMat_; + stateKinematicsInitCovMat_.block(angVelIndexTangent(), angVelIndexTangent()) = + stateAngVelInitCovMat_; + + stateKinematicsProcessCovMat_.setZero(); + stateKinematicsProcessCovMat_.block(posIndexTangent(), posIndexTangent()) = statePosProcessCovMat_; + stateKinematicsProcessCovMat_.block(oriIndexTangent(), oriIndexTangent()) = + stateOriProcessCovMat_; + stateKinematicsProcessCovMat_.block(linVelIndexTangent(), linVelIndexTangent()) = + stateLinVelProcessCovMat_; + stateKinematicsProcessCovMat_.block(angVelIndexTangent(), angVelIndexTangent()) = + stateAngVelProcessCovMat_; + + // ekf_.setStateCovariance(ekf_.getPmatrixZero()); + ekf_.setQ(ekf_.getQmatrixZero()); + ekf_.setR(ekf_.getRmatrixZero()); + + resetStateCovarianceMat(); + resetProcessCovarianceMat(); +} + +const Vector6 KineticsObserver::getCentroidContactWrench(const int & numContact) const +{ + Vector6 centroidContactWrench; + centroidContactWrench.segment(0) = + contacts_.at(numContact).centroidContactKine.orientation.toMatrix3() + * worldCentroidStateVector_.segment(contactForceIndex(numContact)); + + centroidContactWrench.segment(sizeForce) = + contacts_.at(numContact).centroidContactKine.orientation.toMatrix3() + * worldCentroidStateVector_.segment(contactTorqueIndex(numContact)) + + contacts_.at(numContact).centroidContactKine.position().cross(centroidContactWrench.segment(0)); + + return centroidContactWrench; +} + +const kine::Kinematics KineticsObserver::getCentroidContactInputPose(const int & numContact) const +{ + return contacts_.at(numContact).centroidContactKine; +} + +const kine::Kinematics KineticsObserver::getWorldContactInputRefPose(const int & numContact) const +{ + return contacts_.at(numContact).worldRefPose_DEBUG; +} + +const kine::Kinematics KineticsObserver::getWorldContactPose(const int & numContact) const +{ + Kinematics worldContactPose; + worldContactPose.setToProductNoAlias(Kinematics(worldCentroidStateKinematics_), + contacts_.at(numContact).centroidContactKine); + return worldContactPose; +} + +const kine::Kinematics & KineticsObserver::getContactStateRestKinematics(const int & numContact) const +{ + return contacts_.at(numContact).worldRestPose; +} + +const kine::Kinematics KineticsObserver::getUserContactInputPose(const int & numContact) const +{ + return contacts_.at(numContact).userContactKine; +} + +const Vector6 KineticsObserver::getWorldContactWrench(const int & numContact) const +{ + Vector6 worldContactWrench; + worldContactWrench.segment(0) = + worldCentroidStateKinematics_.orientation.toMatrix3() + * contacts_.at(numContact).centroidContactKine.orientation.toMatrix3() + * worldCentroidStateVector_.segment(contactForceIndex(numContact)); + + Kinematics worldContactKine; + worldContactKine.setToProductNoAlias(Kinematics(worldCentroidStateKinematics_), + contacts_.at(numContact).centroidContactKine); + worldContactWrench.segment(sizeForce) = + worldContactKine.orientation.toMatrix3() + * worldCentroidStateVector_.segment(contactTorqueIndex(numContact)) + + worldContactKine.position().cross(worldContactWrench.segment(0)); + + return worldContactWrench; +} + +const int KineticsObserver::getIMUMeasIndexByNum(const int & num) const +{ + return imuSensors_[num].measIndex; +} + +const int KineticsObserver::getContactMeasIndexByNum(const int & num) const +{ + return contacts_[num].measIndex; +} + +const bool KineticsObserver::getContactIsSetByNum(const int & num) const +{ + if(num >= contacts_.size() || contacts_.size() == 0) + { + return false; + } + else + { + return contacts_[num].isSet; + } +} + +const double & KineticsObserver::getMass() const +{ + return mass_; +} + +const IndexedMatrix3 & KineticsObserver::getInertiaMatrix() const +{ + return I_; +} + +const IndexedMatrix3 & KineticsObserver::getInertiaMatrixDot() const +{ + return Id_; +} + +const IndexedVector3 & KineticsObserver::getAngularMomentum() const +{ + return sigma_; +} + +const IndexedVector3 & KineticsObserver::getAngularMomentumDot() const +{ + return sigmad_; +} + +const IndexedVector3 & KineticsObserver::getCenterOfMass() const +{ + return com_; +} + +const IndexedVector3 & KineticsObserver::getCenterOfMassDot() const +{ + return comd_; +} + +const IndexedVector3 & KineticsObserver::getCenterOfMassDotDot() const +{ + return comdd_; +} + +const Vector6 KineticsObserver::getAdditionalWrench() const +{ + Vector6 additionalWrench; + additionalWrench.segment(0) = additionalForce_; + additionalWrench.segment(sizeForce) = additionalTorque_; + return additionalWrench; +} + } // namespace stateObservation diff --git a/src/tilt-estimator.cpp b/src/tilt-estimator.cpp index 6d6c5d53..bd103fc3 100644 --- a/src/tilt-estimator.cpp +++ b/src/tilt-estimator.cpp @@ -9,6 +9,17 @@ TiltEstimator::TiltEstimator(double alpha, double beta, double gamma) { } +void TiltEstimator::initEstimator(Vector3 x1, Vector3 x2_, Vector3 x2) +{ + Eigen::VectorXd initStateVector = Eigen::VectorXd::Zero(getStateSize()); + + initStateVector.segment<3>(0) = x1; + initStateVector.segment<3>(3) = x2_; + initStateVector.segment<3>(6) = x2; + + setState(initStateVector, 0); +} + void TiltEstimator::setMeasurement(const Vector3 ya_k, const Vector3 yg_k, TimeIndex k) { ObserverBase::MeasureVector y_k(6); @@ -17,6 +28,12 @@ void TiltEstimator::setMeasurement(const Vector3 ya_k, const Vector3 yg_k, TimeI ZeroDelayObserver::setMeasurement(y_k, k); } +void TiltEstimator::setExplicitX1(const Vector3 & x1) +{ + withExplicitX1_ = true; + x1_ = x1; +} + ObserverBase::StateVector TiltEstimator::oneStepEstimation_() { TimeIndex k = this->x_.getTime(); @@ -26,9 +43,16 @@ ObserverBase::StateVector TiltEstimator::oneStepEstimation_() Vector3 ya = getMeasurement(k + 1).head<3>(); Vector3 yg = getMeasurement(k + 1).tail<3>(); - x1_ = R_S_C_.transpose() * (v_C_ + v_S_C_) + (yg - R_S_C_.transpose() * w_S_C_).cross(R_S_C_.transpose() * p_S_C_); + if(!withExplicitX1_) + { + x1_ = R_S_C_.transpose() * (v_C_ + v_S_C_) + (yg - R_S_C_.transpose() * w_S_C_).cross(R_S_C_.transpose() * p_S_C_); + } + else + { + withExplicitX1_ = false; + } - ObserverBase::StateVector x_hat = getEstimatedState(k); + ObserverBase::StateVector x_hat = getCurrentEstimatedState(); x1_hat_ = x_hat.segment<3>(0); x2_hat_prime_ = x_hat.segment<3>(3); x2_hat_ = x_hat.segment<3>(6); diff --git a/unit-testings/CMakeLists.txt b/unit-testings/CMakeLists.txt index b530d777..ea3c0fe7 100755 --- a/unit-testings/CMakeLists.txt +++ b/unit-testings/CMakeLists.txt @@ -1,6 +1,7 @@ macro(ADD_SO_TEST name) add_executable(${name} ${name}.cpp) - target_link_libraries(${name} PUBLIC state-observation ${ARGN}) + + target_link_libraries(${name} PUBLIC state-observation benchmark ${ARGN}) add_test( NAME ${name} @@ -22,6 +23,9 @@ add_so_test(test_model-base-ekf-flex-estimator-imu) add_so_test(test-kinetics-observer) add_so_test(test-dcm-bias-estimation) add_so_test(test-kinematics) +add_so_test(test-local-kinematics) +add_so_test(test-runge-kutta) +add_so_test(test-analytical-jacobians) add_custom_command( TARGET test_model-base-ekf-flex-estimator-imu diff --git a/unit-testings/test-analytical-jacobians.cpp b/unit-testings/test-analytical-jacobians.cpp new file mode 100644 index 00000000..557d399a --- /dev/null +++ b/unit-testings/test-analytical-jacobians.cpp @@ -0,0 +1,734 @@ +#include +#include +#include + +#include +#include +#include + +using namespace stateObservation::kine; + +namespace stateObservation +{ + +double dt_ = 0.005; + +double lin_stiffness_ = (double)rand() / RAND_MAX * 1e5; +double lin_damping_ = (double)rand() / RAND_MAX * 5 * 1e1; +double ang_stiffness_ = (double)rand() / RAND_MAX * 1e5; +double ang_damping_ = (double)rand() / RAND_MAX * 5 * 1e1; + +Matrix3 K1_ = lin_stiffness_ * Matrix3::Identity(); +Matrix3 K2_ = lin_damping_ * Matrix3::Identity(); +Matrix3 K3_ = ang_stiffness_ * Matrix3::Identity(); +Matrix3 K4_ = ang_damping_ * Matrix3::Identity(); + +double lin_stiffness_2_ = (double)rand() / RAND_MAX * 1e5; +double lin_damping_2_ = (double)rand() / RAND_MAX * 5 * 1e1; +double ang_stiffness_2_ = (double)rand() / RAND_MAX * 1e5; +double ang_damping_2_ = (double)rand() / RAND_MAX * 5 * 1e1; + +Matrix3 K1_2_ = lin_stiffness_2_ * Matrix3::Identity(); +Matrix3 K2_2_ = lin_damping_2_ * Matrix3::Identity(); +Matrix3 K3_2_ = ang_stiffness_2_ * Matrix3::Identity(); +Matrix3 K4_2_ = ang_damping_2_ * Matrix3::Identity(); + +Vector3 com_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 com_d_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 com_dd_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + +Vector3 position_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation ori_; +Vector3 linvel_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 angvel_ = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10 * 100; + +Vector3 gyroBias1_ = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; +Vector3 gyroBias2_ = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; + +Vector3 extForces_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 extTorques_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + +Vector3 worldContactPos1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation worldContactOri1_; +Vector3 centroidContactPos1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation centroidContactOri1_; +Vector3 centroidContactLinVel1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidContactAngVel1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 contactForces1_ = tools::ProbabilityLawSimulation::getUniformMatrix() * 1000; +Vector3 contactTorques1_ = tools::ProbabilityLawSimulation::getUniformMatrix() * 10; + +Vector3 worldContactPos2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation worldContactOri2_; +Vector3 centroidContactPos2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation centroidContactOri2_; +Vector3 centroidContactLinVel2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidContactAngVel2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 contactForces2_ = tools::ProbabilityLawSimulation::getUniformMatrix() * 100; +Vector3 contactTorques2_ = tools::ProbabilityLawSimulation::getUniformMatrix() * 10; + +Vector3 centroidIMUPos1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation centroidIMUOri1_; +Vector3 centroidIMULinVel1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidIMUAngVel1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidIMULinAcc1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidIMUAngAcc1_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + +Vector3 centroidIMUPos2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +kine::Orientation centroidIMUOri2_; +Vector3 centroidIMULinVel2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidIMUAngVel2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidIMULinAcc2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 centroidIMUAngAcc2_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + +Matrix3 inertiaMatrix_ = tools::ProbabilityLawSimulation::getUniformMatrix(); +Matrix3 inertiaMatrix_d_ = tools::ProbabilityLawSimulation::getGaussianMatrix(); +Vector3 angularMomentum_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; +Vector3 angularMomentum_d_ = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + +Eigen::IOFormat CleanFmt_(4, 0, ", ", "\n", "[", "]"); + +Vector dx_; +double error_ = 0; + +KineticsObserver ko_1_(1, 1); +KineticsObserver ko_2_(2, 2); + +/////////////////////////////////////////////////////////////////////// +/// -------------------Intermediary functions for the tests------------- +/////////////////////////////////////////////////////////////////////// + +Matrix displayVectorWithIndex(Matrix A) // to be remove +{ + Matrix indexedA(A.rows() + 1, A.cols() + 1); + indexedA.setZero(); + indexedA.block(1, 1, A.rows(), A.cols()) = A; + + for(int i = 0; i < A.rows(); i++) + { + for(int j = 0; j < A.cols(); j++) + { + indexedA(0, j + 1) = j; + indexedA(i + 1, 0) = i; + } + } + return indexedA; +} + +/////////////////////////////////////////////////////////////////////// +/// -------------------Tests implementation------------- +/////////////////////////////////////////////////////////////////////// + +int testAccelerationsJacobians(KineticsObserver & ko_, + int errcode, + double relativeErrorThreshold, + double threshold) // 1 +{ + /* Finite differences Jacobian */ + Matrix accJacobianFD = Matrix::Zero(6, ko_.getStateTangentSize()); + + Vector accBar = Vector6::Zero(); + Vector accBarIncremented = Vector6::Zero(); + Vector accBarDiff = Vector6::Zero(); + + Vector x = ko_.getEKF().getCurrentEstimatedState(); + Vector xIncrement = ko_.getEKF().getCurrentEstimatedState(); + + ko_.computeLocalAccelerations(x, accBar); + + xIncrement.resize(ko_.getStateTangentSize()); + + for(Index i = 0; i < ko_.getStateTangentSize(); ++i) + { + xIncrement.setZero(); + xIncrement[i] = dx_[i]; + + ko_.stateSum(x, xIncrement, x); + + ko_.computeLocalAccelerations(x, accBarIncremented); + + accBarDiff = accBarIncremented - accBar; + + accBarDiff /= dx_[i]; + + accJacobianFD.col(i) = accBarDiff; + + x = ko_.getEKF().getCurrentEstimatedState(); + } + + /* Analytical jacobian */ + + LocalKinematics worldCentroidKinematics(x, ko_.flagsStateKine); + Matrix accJacobianAnalytical = Matrix::Zero(6, ko_.getStateTangentSize()); + Matrix3 I_inv = ko_.getInertiaMatrix()().inverse(); + + // Jacobians of the linear acceleration + accJacobianAnalytical.block<3, ko_.sizeOriTangent>(0, ko_.oriIndexTangent()) = + -cst::gravityConstant + * (worldCentroidKinematics.orientation.toMatrix3().transpose() * kine::skewSymmetric(Vector3(0, 0, 1))); + accJacobianAnalytical.block<3, ko_.sizeForceTangent>(0, ko_.unmodeledForceIndexTangent()) = + Matrix::Identity(ko_.sizeLinAccTangent, ko_.sizeTorqueTangent) / ko_.getMass(); + + // Jacobians of the angular acceleration + accJacobianAnalytical.block<3, ko_.sizeTorqueTangent>(3, ko_.unmodeledTorqueIndexTangent()) = I_inv; + accJacobianAnalytical.block<3, ko_.sizeAngVelTangent>(3, ko_.angVelIndexTangent()) = + I_inv + * (kine::skewSymmetric(ko_.getInertiaMatrix()() * worldCentroidKinematics.angVel()) - ko_.getInertiaMatrixDot()() + - kine::skewSymmetric(worldCentroidKinematics.angVel()) * ko_.getInertiaMatrix()() + + kine::skewSymmetric(ko_.getAngularMomentum()())); + + // Jacobians with respect to the contacts + for(KineticsObserver::VectorContactConstIterator i = ko_.contacts_.begin(); i != ko_.contacts_.end(); ++i) + { + if(i->isSet) + { + // Jacobian of the linar acceleration with respect to the contact force + accJacobianAnalytical.block<3, ko_.sizeForceTangent>(0, ko_.contactForceIndexTangent(i)) = + (1.0 / ko_.getMass()) * i->centroidContactKine.orientation.toMatrix3(); + // Jacobian of the angular acceleration with respect to the contact force + accJacobianAnalytical.block<3, ko_.sizeTorqueTangent>(3, ko_.contactForceIndexTangent(i)) = + (I_inv * kine::skewSymmetric(i->centroidContactKine.position())) + * (i->centroidContactKine.orientation).toMatrix3(); + // Jacobian of the angular acceleration with respect to the contact torque + accJacobianAnalytical.block<3, ko_.sizeTorqueTangent>(3, ko_.contactTorqueIndexTangent(i)) = + I_inv * i->centroidContactKine.orientation.toMatrix3(); + } + } + + /* Comparison */ + + for(int i = 0; i < accJacobianAnalytical.rows(); i++) + { + for(int j = 0; j < accJacobianAnalytical.cols(); j++) + { + if(abs(accJacobianAnalytical(i, j) - accJacobianFD(i, j)) + / std::max(abs(accJacobianAnalytical(i, j)), abs(accJacobianFD(i, j))) * 100 + > relativeErrorThreshold + && abs(accJacobianAnalytical(i, j) - accJacobianFD(i, j)) != 0 + && (abs(accJacobianAnalytical(i, j)) > 1.0e-9 && abs(accJacobianFD(i, j)) > 1.0e-9)) + { + std::cout << std::endl + << "\033[1;31m" + << "error indexes: " << std::endl + << "(" << i << "," << j << "): Analytic : " << accJacobianAnalytical(i, j) + << " FD : " << accJacobianFD(i, j) << " Relative error : " + << abs(accJacobianAnalytical(i, j) - accJacobianFD(i, j)) + / std::max(abs(accJacobianAnalytical(i, j)), abs(accJacobianFD(i, j))) * 100 + << " % " + << "\033[0m\n" + << std::endl; + } + } + } + + error_ = (accJacobianAnalytical - accJacobianFD).squaredNorm(); + + std::cout << "Error between the analytical and the finite differences acceleration Jacobians: " << error_ + << std::endl; + + if(error_ > threshold) + { + return errcode; + } +} + +int testOrientationsJacobians(KineticsObserver & ko_, int errcode, double relativeErrorThreshold, double threshold) // 2 +{ + /* Finite differences Jacobian */ + Matrix rotationJacobianDeltaFD = Matrix::Zero(3, 3); + + Vector currentState = ko_.getEKF().getCurrentEstimatedState(); + Vector accelerations = Vector6::Zero(); + ko_.computeLocalAccelerations(currentState, accelerations); + LocalKinematics kineTestOri(currentState); + + kineTestOri.linAcc = accelerations.segment<3>(0); + kineTestOri.angAcc = accelerations.segment<3>(3); + + Orientation oriBar = Orientation::zeroRotation(); + Orientation oriBarIncremented = Orientation::zeroRotation(); + Vector3 oriBarDiff = Vector3::Zero(); + + oriBar = kineTestOri.orientation; + oriBarIncremented = kineTestOri.orientation; + + // Vector3 dt_x_omega(10000, 24265, 589); + Vector3 dt_x_omega = dt_ * kineTestOri.angVel() + dt_ * dt_ / 2 * kineTestOri.angAcc(); + + oriBar.integrateRightSide(dt_x_omega); + + Vector3 xIncrement = Vector3::Zero(); + + for(Index i = 0; i < 3; ++i) + { + xIncrement.setZero(); + xIncrement[i] = dx_[i]; + + Vector3 incremented_dt_x_omega = dt_x_omega + xIncrement; + + oriBarIncremented.integrateRightSide(incremented_dt_x_omega); + + oriBarDiff = oriBar.differentiate(oriBarIncremented); + + oriBarDiff /= dx_[i]; + + rotationJacobianDeltaFD.col(i) = oriBarDiff; + oriBarIncremented = kineTestOri.orientation; + } + + Matrix rotationJacobianDeltaAnalytical = + 2.0 / dt_x_omega.norm() + * (((dt_x_omega.norm() - 2.0 * sin(dt_x_omega.norm() / 2.0)) / (2.0 * dt_x_omega.squaredNorm())) + * kineTestOri.orientation.toMatrix3() * dt_x_omega * dt_x_omega.transpose() + + sin(dt_x_omega.norm() / 2.0) * kineTestOri.orientation.toMatrix3() + * kine::rotationVectorToRotationMatrix(dt_x_omega / 2.0)); + + for(int i = 0; i < rotationJacobianDeltaAnalytical.rows(); i++) + { + for(int j = 0; j < rotationJacobianDeltaAnalytical.cols(); j++) + { + if(abs(rotationJacobianDeltaAnalytical(i, j) - rotationJacobianDeltaFD(i, j)) + / std::max(abs(rotationJacobianDeltaAnalytical(i, j)), abs(rotationJacobianDeltaFD(i, j))) * 100 + > relativeErrorThreshold + && abs(rotationJacobianDeltaAnalytical(i, j) - rotationJacobianDeltaFD(i, j)) != 0) + { + std::cout << std::endl + << "\033[1;31m" + << "error indexes: " << std::endl + << "(" << i << "," << j << "): Analytic : " << rotationJacobianDeltaAnalytical(i, j) + << " FD : " << rotationJacobianDeltaFD(i, j) << " Relative error : " + << abs(rotationJacobianDeltaAnalytical(i, j) - rotationJacobianDeltaFD(i, j)) + / std::max(abs(rotationJacobianDeltaAnalytical(i, j)), abs(rotationJacobianDeltaFD(i, j))) + * 100 + << " % " + << "\033[0m\n" + << std::endl; + } + } + } + + error_ = (rotationJacobianDeltaAnalytical - rotationJacobianDeltaFD).squaredNorm(); + + std::cout << "Error between the analytical and the finite differences Jacobians of the orientation integration wrt " + "an increment delta: " + << error_ << std::endl; + + if(error_ > threshold) + { + return errcode; + } +} + +int testAnalyticalAJacobianVsFD(KineticsObserver & ko_, + int errcode, + double relativeErrorThreshold, + double threshold) // 3 +{ + Matrix A_analytic = ko_.computeAMatrix(); + + Matrix A_FD = ko_.getEKF().getAMatrixFD(dx_); + + for(int i = 0; i < A_analytic.rows(); i++) + { + for(int j = 0; j < A_analytic.cols(); j++) + { + if(abs(A_analytic(i, j) - A_FD(i, j)) / std::max(abs(A_analytic(i, j)), abs(A_FD(i, j))) * 100 + > relativeErrorThreshold + && abs(A_analytic(i, j) - A_FD(i, j)) != 0 && (abs(A_analytic(i, j)) > 1.0e-9 && abs(A_FD(i, j)) > 1.0e-9)) + { + std::cout << std::endl + << "\033[1;31m" + << "error indexes: " << std::endl + << "(" << i << "," << j << "): Analytic : " << A_analytic(i, j) << " FD : " << A_FD(i, j) + << " Relative error : " + << abs(A_analytic(i, j) - A_FD(i, j)) / std::max(abs(A_analytic(i, j)), abs(A_FD(i, j))) * 100 + << " % " + << "\033[0m\n" + << std::endl; + } + else + { + /* + std::cout << std::endl + << "good indexes: " << std::endl + << "(" << i << "," << j << "): Analytic : " << A_analytic(i, j) << " FD : " << A_FD(i, j) + << " Relative error : " << abs(A_analytic(i, j) - A_FD(i, j)) / std::max(abs(A_analytic(i, + j)), abs(A_FD(i, j))) * 100 + << " % " << std::endl; + + */ + } + } + } + + error_ = (A_analytic - A_FD).squaredNorm(); + + std::cout << "Error between the analytical and the finite differences A Jacobian: " << error_ << std::endl; + + if(error_ > threshold) + { + return errcode; + } +} + +int testAnalyticalCJacobianVsFD(KineticsObserver & ko_, + int errcode, + double relativeErrorThreshold, + double threshold) // 3 +{ + Matrix C_analytic = ko_.computeCMatrix(); + + Matrix C_FD = ko_.getEKF().getCMatrixFD(dx_); + + for(int i = 0; i < C_analytic.rows(); i++) + { + for(int j = 0; j < C_analytic.cols(); j++) + { + if(abs(C_analytic(i, j) - C_FD(i, j)) / std::max(abs(C_analytic(i, j)), abs(C_FD(i, j))) * 100 + > relativeErrorThreshold + && abs(C_analytic(i, j) - C_FD(i, j)) != 0 && (abs(C_analytic(i, j)) > 1.0e-9 && abs(C_FD(i, j)) > 1.0e-9)) + { + std::cout << std::endl + << "\033[1;31m" + << "error indexes: " << std::endl + << "(" << i << "," << j << "): Analytic : " << C_analytic(i, j) << " FD : " << C_FD(i, j) + << " Relative error : " + << abs(C_analytic(i, j) - C_FD(i, j)) / std::max(abs(C_analytic(i, j)), abs(C_FD(i, j))) * 100 + << " % " + << "\033[0m\n" + << std::endl; + } + else + { + /* + std::cout << std::endl + << "good indexes: " << std::endl + << "(" << i << "," << j << "): C_analytic : " << C_analytic(i, j) << " FD : " << C_FD(i, j) + << " Relative error : " << abs(C_analytic(i, j) - C_FD(i, j)) / std::max(abs(C_analytic(i, + j)), abs(C_FD(i, j))) * 100 + << " % " << std::endl; + + */ + } + } + } + + error_ = (C_analytic - C_FD).squaredNorm(); + + std::cout << "Error between the analytical and the finite differences C Jacobian: " << error_ << std::endl; + + if(error_ > threshold) + { + return errcode; + } +} + +} // end namespace stateObservation + +using namespace stateObservation; + +int main() +{ + int returnVal; + int errorcode = 0; + + Vector stateVector_; + + inertiaMatrix_ = inertiaMatrix_ * inertiaMatrix_.transpose(); + inertiaMatrix_d_ = inertiaMatrix_d_ * inertiaMatrix_d_.transpose(); + + ori_.setRandom(); + + /* Kinetics Observer 1 initialization */ + + worldContactOri1_.setRandom(); + Kinematics worldContactPose1_; + worldContactPose1_.position = worldContactPos1_; + worldContactPose1_.orientation = worldContactOri1_; + centroidContactOri1_.setRandom(); + Kinematics centroidContactPose1_; + centroidContactPose1_.position = centroidContactPos1_; + centroidContactPose1_.orientation = centroidContactOri1_; + centroidContactPose1_.linVel = centroidContactLinVel1_; + centroidContactPose1_.angVel = centroidContactAngVel1_; + + centroidIMUOri1_.setRandom(); + Kinematics centroidIMUPose1_; + centroidIMUPose1_.position = centroidIMUPos1_; + centroidIMUPose1_.orientation = centroidIMUOri1_; + centroidIMUPose1_.linVel = centroidIMULinVel1_; + centroidIMUPose1_.angVel = centroidIMUAngVel1_; + centroidIMUPose1_.linAcc = centroidIMULinAcc1_; + centroidIMUPose1_.angAcc = centroidIMUAngAcc1_; + + ko_1_.setCenterOfMass(com_, com_d_, com_dd_); + + ko_1_.setSamplingTime(dt_); + ko_1_.setWithUnmodeledWrench(true); + ko_1_.useRungeKutta(false); + ko_1_.setWithGyroBias(true); + + ko_1_.setCoMAngularMomentum(angularMomentum_, angularMomentum_d_); + ko_1_.setCoMInertiaMatrix(inertiaMatrix_, inertiaMatrix_d_); + + ko_1_.addContact(worldContactPose1_, 0, K1_, K2_, K3_, K4_); + ko_1_.updateContactWithWrenchSensor(Vector6::Zero(), centroidContactPose1_, 0); + + ko_1_.setIMU(Vector3::Zero(), Vector3::Zero(), centroidIMUPose1_, 0); + + stateVector_.resize(position_.size() + 4 + linvel_.size() + angvel_.size() + gyroBias1_.size() + extForces_.size() + + extTorques_.size() + worldContactPos1_.size() + worldContactOri1_.toVector4().size() + + contactForces1_.size() + contactTorques1_.size()); + stateVector_ << position_, ori_.toVector4(), linvel_, angvel_, gyroBias1_, extForces_, extTorques_, worldContactPos1_, + worldContactOri1_.toVector4(), contactForces1_, contactTorques1_; + + ko_1_.setInitWorldCentroidStateVector(stateVector_); + + dx_.resize(ko_1_.getStateTangentSize()); + dx_.setZero(); + dx_.setConstant(1e-6); + + dx_.segment(ko_1_.contactForceIndexTangent(0)).setConstant(1e-5); + + ko_1_.updateMeasurements(); + + ko_1_.getEKF().updateStatePrediction(); + + std::cout << std::endl << "Tests with 1 contact and 1 gyrometer: " << std::endl << std::endl; + + /* + for(int i = 0; i < stateVector_.size(); i++) + { + dx_.segment(ko_.posIndexTangent()) + .setConstant(stateVector_.segment(ko_.posIndex()).norm() * 1e-8); + dx_.segment(ko_.oriIndexTangent()) + .setConstant(stateVector_.segment(ko_.oriIndex()).norm() * 1e-8); + dx_.segment(ko_.linVelIndexTangent()) + .setConstant(stateVector_.segment(ko_.linVelIndex()).norm() * 1e-8); + dx_.segment(ko_.angVelIndexTangent()) + .setConstant(stateVector_.segment(ko_.angVelIndex()).norm() * 1e-8); + dx_.segment(ko_.gyroBiasIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.gyroBiasIndex(0)).norm() * 1e-8); + dx_.segment(ko_.unmodeledForceIndexTangent()) + .setConstant(stateVector_.segment(ko_.unmodeledForceIndex()).norm() * 1e-8); + dx_.segment(ko_.unmodeledTorqueIndexTangent()) + .setConstant(stateVector_.segment(ko_.unmodeledTorqueIndex()).norm() * 1e-8); + dx_.segment(ko_.contactPosIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactPosIndex(0)).norm() * 1e-8); + dx_.segment(ko_.contactOriIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactOriIndex(0)).norm() * 1e-8); + dx_.segment(ko_.contactForceIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactForceIndex(0)).norm() * 1e-8); + dx_.segment(ko_.contactTorqueIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactTorqueIndex(0)).norm() * 1e-8); + if(secondContactAndGyro_) + { + dx_.segment(ko_.gyroBiasIndexTangent(1)) + .setConstant(stateVector_.segment(ko_.gyroBiasIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactPosIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactPosIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactOriIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactOriIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactForceIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactForceIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactTorqueIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactTorqueIndex(1)).norm() * 1e-8); + } + } + */ + std::cout << "Starting testAccelerationsJacobians." << std::endl; + if((returnVal = testAccelerationsJacobians(ko_1_, ++errorcode, 0.1, 5e-11))) + { + std::cout << "testAccelerationsJacobians Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testAccelerationsJacobians succeeded" << std::endl; + } + + std::cout << "Starting testOrientationsJacobians." << std::endl; + if((returnVal = testOrientationsJacobians(ko_1_, ++errorcode, 0.1, 1.66e-16))) + { + std::cout << "testOrientationsJacobians Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testOrientationsJacobians succeeded" << std::endl; + } + + std::cout << "Starting testAnalyticalAJacobianVsFD." << std::endl; + if((returnVal = testAnalyticalAJacobianVsFD(ko_1_, ++errorcode, 2.5, 0.005))) + { + std::cout << "testAnalyticalAJacobianVsFD Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testAnalyticalAJacobianVsFD succeeded" << std::endl; + } + + std::cout << "Starting testAnalyticalCJacobianVsFD." << std::endl; + if((returnVal = testAnalyticalCJacobianVsFD(ko_1_, ++errorcode, 0.05, 9.9e-11))) + { + std::cout << "testAnalyticalCJacobianVsFD Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testAnalyticalCJacobianVsFD succeeded" << std::endl; + } + + /* Kinetics Observer 2 initialization */ + + worldContactOri2_.setRandom(); + Kinematics worldContactPose2_; + worldContactPose2_.position = worldContactPos2_; + worldContactPose2_.orientation = worldContactOri2_; + centroidContactOri2_.setRandom(); + Kinematics centroidContactPose2_; + centroidContactPose2_.position = centroidContactPos2_; + centroidContactPose2_.orientation = centroidContactOri2_; + centroidContactPose2_.linVel = centroidContactLinVel2_; + centroidContactPose2_.angVel = centroidContactAngVel2_; + + centroidIMUOri2_.setRandom(); + Kinematics centroidIMUPose2_; + centroidIMUPose2_.position = centroidIMUPos2_; + centroidIMUPose2_.orientation = centroidIMUOri2_; + centroidIMUPose2_.linVel = centroidIMULinVel2_; + centroidIMUPose2_.angVel = centroidIMUAngVel2_; + centroidIMUPose2_.linAcc = centroidIMULinAcc2_; + centroidIMUPose2_.angAcc = centroidIMUAngAcc2_; + + ko_2_.setCenterOfMass(com_, com_d_, com_dd_); + + ko_2_.setSamplingTime(dt_); + ko_2_.setWithUnmodeledWrench(true); + ko_2_.useRungeKutta(false); + ko_2_.setWithGyroBias(true); + + ko_2_.setCoMAngularMomentum(angularMomentum_, angularMomentum_d_); + + ko_2_.setCoMInertiaMatrix(inertiaMatrix_, inertiaMatrix_d_); + + ko_2_.addContact(worldContactPose1_, 0, K1_, K2_, K3_, K4_); + ko_2_.updateContactWithWrenchSensor(Vector6::Zero(), centroidContactPose1_, 0); + ko_2_.addContact(worldContactPose2_, 1, K1_2_, K2_2_, K3_2_, K4_2_); + ko_2_.updateContactWithWrenchSensor(Vector6::Zero(), centroidContactPose2_, 1); + + ko_2_.setIMU(Vector3::Zero(), Vector3::Zero(), centroidIMUPose1_, 0); + ko_2_.setIMU(Vector3::Zero(), Vector3::Zero(), centroidIMUPose2_, 1); + + stateVector_.resize(position_.size() + 4 + linvel_.size() + angvel_.size() + gyroBias1_.size() + gyroBias2_.size() + + extForces_.size() + extTorques_.size() + worldContactPos1_.size() + + worldContactOri1_.toVector4().size() + contactForces1_.size() + contactTorques1_.size() + + worldContactPos2_.size() + worldContactOri2_.toVector4().size() + contactForces2_.size() + + contactTorques2_.size()); + stateVector_ << position_, ori_.toVector4(), linvel_, angvel_, gyroBias1_, gyroBias2_, extForces_, extTorques_, + worldContactPos1_, worldContactOri1_.toVector4(), contactForces1_, contactTorques1_, worldContactPos2_, + worldContactOri2_.toVector4(), contactForces2_, contactTorques2_; + + ko_2_.setInitWorldCentroidStateVector(stateVector_); + + dx_.resize(ko_2_.getStateTangentSize()); + dx_.setZero(); + dx_.setConstant(1e-6); + + dx_.segment(ko_2_.contactForceIndexTangent(0)).setConstant(1e-5); + dx_.segment(ko_2_.contactForceIndexTangent(1)).setConstant(1e-5); + + ko_2_.updateMeasurements(); + + ko_2_.getEKF().updateStatePrediction(); + + std::cout << std::endl << "Tests with 2 contacts and 2 gyrometers: " << std::endl << std::endl; + + /* + for(int i = 0; i < stateVector_.size(); i++) + { + dx_.segment(ko_.posIndexTangent()) + .setConstant(stateVector_.segment(ko_.posIndex()).norm() * 1e-8); + dx_.segment(ko_.oriIndexTangent()) + .setConstant(stateVector_.segment(ko_.oriIndex()).norm() * 1e-8); + dx_.segment(ko_.linVelIndexTangent()) + .setConstant(stateVector_.segment(ko_.linVelIndex()).norm() * 1e-8); + dx_.segment(ko_.angVelIndexTangent()) + .setConstant(stateVector_.segment(ko_.angVelIndex()).norm() * 1e-8); + dx_.segment(ko_.gyroBiasIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.gyroBiasIndex(0)).norm() * 1e-8); + dx_.segment(ko_.unmodeledForceIndexTangent()) + .setConstant(stateVector_.segment(ko_.unmodeledForceIndex()).norm() * 1e-8); + dx_.segment(ko_.unmodeledTorqueIndexTangent()) + .setConstant(stateVector_.segment(ko_.unmodeledTorqueIndex()).norm() * 1e-8); + dx_.segment(ko_.contactPosIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactPosIndex(0)).norm() * 1e-8); + dx_.segment(ko_.contactOriIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactOriIndex(0)).norm() * 1e-8); + dx_.segment(ko_.contactForceIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactForceIndex(0)).norm() * 1e-8); + dx_.segment(ko_.contactTorqueIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactTorqueIndex(0)).norm() * 1e-8); + if(secondContactAndGyro_) + { + dx_.segment(ko_.gyroBiasIndexTangent(1)) + .setConstant(stateVector_.segment(ko_.gyroBiasIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactPosIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactPosIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactOriIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactOriIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactForceIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactForceIndex(1)).norm() * 1e-8); + dx_.segment(ko_.contactTorqueIndexTangent(0)) + .setConstant(stateVector_.segment(ko_.contactTorqueIndex(1)).norm() * 1e-8); + } + } + */ + + std::cout << "Starting testAccelerationsJacobians." << std::endl; + if((returnVal = testAccelerationsJacobians(ko_2_, ++errorcode, 0.1, 4.95e-11))) + { + std::cout << "testAccelerationsJacobians Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testAccelerationsJacobians succeeded" << std::endl; + } + + std::cout << "Starting testOrientationsJacobians." << std::endl; + if((returnVal = testOrientationsJacobians(ko_2_, ++errorcode, 0.1, 1.64e-16))) + { + std::cout << "testOrientationsJacobians Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testOrientationsJacobians succeeded" << std::endl; + } + + std::cout << "Starting testAnalyticalAJacobianVsFD." << std::endl; + if((returnVal = testAnalyticalAJacobianVsFD(ko_2_, ++errorcode, 2.5, 6))) + { + std::cout << "testAnalyticalAJacobianVsFD Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testAnalyticalAJacobianVsFD succeeded" << std::endl; + } + + std::cout << "Starting testAnalyticalCJacobianVsFD." << std::endl; + if((returnVal = testAnalyticalCJacobianVsFD(ko_2_, ++errorcode, 0.77, 1.6e-10))) + { + std::cout << "testAnalyticalCJacobianVsFD Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testAnalyticalCJacobianVsFD succeeded" << std::endl; + } + + std::cout << "test succeeded" << std::endl; + return 0; +} \ No newline at end of file diff --git a/unit-testings/test-kinematics.cpp b/unit-testings/test-kinematics.cpp index 1bfef04c..41ccb308 100644 --- a/unit-testings/test-kinematics.cpp +++ b/unit-testings/test-kinematics.cpp @@ -10,6 +10,156 @@ using namespace kine; /// /// @param errorCode /// @return int + +int testSetToDiffNoAliasKinematics(int errcode) +{ + + std::cout << "testSetToDiffNoAliasKinematics test started" << std::endl; + typedef kine::Kinematics::Flags Flags; + + kine::Kinematics k0; + kine::Kinematics k1; + + Flags::Byte flag0 = BOOST_BINARY(000000); + Flags::Byte flag1 = BOOST_BINARY(000000); + + kine::Kinematics k, l, k2; + + int count = int(pow(2, 6) * pow(2, 6)); + double err = 0; + double threshold = 1e-28 * count; + + for(int i = 0; i < count; i++) + { + /* + std::cout << std::endl << "-----------------------------------------" << std::endl + << "New iteration" + << std::endl << "-----------------------------------------" << std::endl; + std::cout << "err: " << err << std::endl; + */ + Vector3 pos0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori0 = kine::Orientation::randomRotation(); + Vector3 linvel0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + Vector3 pos1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori1 = kine::Orientation::randomRotation(); + Vector3 linvel1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel1 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc1 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + k0.reset(); + k1.reset(); + + if(true) /// the position has to be set + { + k0.position = pos0; + } + if(true) /// the orientation has to be set + { + k0.orientation = ori0; + } + if(flag0 & Flags::linVel) + { + k0.linVel = linvel0; + } + if(flag0 & Flags::angVel) + { + k0.angVel = angvel0; + } + if(flag0 & Flags::linAcc) + { + k0.linAcc = linacc0; + } + if(flag0 & Flags::angAcc) + { + k0.angAcc = angacc0; + } + + if(true) /// the position has to be set + { + k1.position = pos1; + } + if(true) /// the orientation has to be set + { + k1.orientation = ori1; + } + if(flag1 & Flags::linVel) + { + k1.linVel = linvel1; + } + if(flag1 & Flags::angVel) + { + k1.angVel = angvel1; + } + if(flag1 & Flags::linAcc) + { + k1.linAcc = linacc1; + } + if(flag1 & Flags::angAcc) + { + k1.angAcc = angacc1; + } + + if((flag0 = (flag0 + 1) & Flags::all) == 0) /// update the flags to span all the possibilties + flag1 = (flag1 + 1) & Flags::all; + + k2 = k1; + if(!k1.orientation.isSet()) + { + k2.orientation.setZeroRotation(); + } + Kinematics k3; + Kinematics k4; + + k3.setToDiffNoAlias(k2, k0); + + k4.setToProductNoAlias(k2, k0.getInverse()); + + // std::cout << std::endl << "k3: " << std::endl << k3 << std::endl; + // std::cout << std::endl << "k4: " << std::endl << k4 << std::endl; + + k = k4 * k3.getInverse(); + + if(k.position.isSet()) + { + err += k.position().squaredNorm(); + } + if(k.orientation.isSet()) + { + err += k.orientation.toRotationVector().squaredNorm(); + } + if(k.linVel.isSet()) + { + err += k.linVel().squaredNorm(); + } + if(k.angVel.isSet()) + { + err += k.angVel().squaredNorm(); + } + if(k.linAcc.isSet()) + { + err += k.linAcc().squaredNorm(); + } + if(k.angAcc.isSet()) + { + err += k.angAcc().squaredNorm(); + } + } + + std::cout << "Error 1 : " << err << std::endl; + + if(err > threshold) + { + std::cout << "Error too large : " << err << std::endl; + return errcode; + } + return 0; +} + int testRotationOperations(int errorCode) { unsigned numberOfTests = 1000; @@ -372,37 +522,37 @@ int testOrientation(int errcode) kine::Orientation ori17(ori3, ori2); err += (ori00.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 1 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 1 done. Current error " << err << std::endl; err += (ori01.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 2 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 2 done. Current error " << err << std::endl; err += (ori02.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 3 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 3 done. Current error " << err << std::endl; err += (ori03.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 4 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 4 done. Current error " << err << std::endl; err += (ori04.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 5 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 5 done. Current error " << err << std::endl; err += (ori05.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 6 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 6 done. Current error " << err << std::endl; err += (ori06.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 7 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 7 done. Current error " << err << std::endl; err += (ori07.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 8 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 8 done. Current error " << err << std::endl; err += (ori10.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 9 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 9 done. Current error " << err << std::endl; err += (ori11.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 10 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 10 done. Current error " << err << std::endl; err += (ori12.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 11 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 11 done. Current error " << err << std::endl; err += (ori13.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 12 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 12 done. Current error " << err << std::endl; err += (ori14.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 13 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 13 done. Current error " << err << std::endl; err += (ori15.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 14 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 14 done. Current error " << err << std::endl; err += (ori16.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 15 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 15 done. Current error " << err << std::endl; err += (ori17.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 16 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 16 done. Current error " << err << std::endl; } { @@ -468,37 +618,37 @@ int testOrientation(int errcode) kine::Orientation ori17(ori3, ori2); err += (ori00.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 17 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 17 done. Current error " << err << std::endl; err += (ori01.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 18 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 18 done. Current error " << err << std::endl; err += (ori02.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 19 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 19 done. Current error " << err << std::endl; err += (ori03.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 20 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 20 done. Current error " << err << std::endl; err += (ori04.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 21 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 21 done. Current error " << err << std::endl; err += (ori05.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 22 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 22 done. Current error " << err << std::endl; err += (ori06.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 23 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 23 done. Current error " << err << std::endl; err += (ori07.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 24 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 24 done. Current error " << err << std::endl; err += (ori10.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 25 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 25 done. Current error " << err << std::endl; err += (ori11.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 26 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 26 done. Current error " << err << std::endl; err += (ori12.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 27 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 27 done. Current error " << err << std::endl; err += (ori13.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 28 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 28 done. Current error " << err << std::endl; err += (ori14.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 29 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 29 done. Current error " << err << std::endl; err += (ori15.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 30 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 30 done. Current error " << err << std::endl; err += (ori16.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 31 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 31 done. Current error " << err << std::endl; err += (ori17.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 32 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 32 done. Current error " << err << std::endl; } { @@ -567,37 +717,37 @@ int testOrientation(int errcode) kine::Orientation ori17(ori3, ori2); err += (ori00.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 33 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 33 done. Current error " << err << std::endl; err += (ori01.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 34 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 34 done. Current error " << err << std::endl; err += (ori02.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 35 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 35 done. Current error " << err << std::endl; err += (ori03.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 36 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 36 done. Current error " << err << std::endl; err += (ori04.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 37 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 37 done. Current error " << err << std::endl; err += (ori05.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 38 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 38 done. Current error " << err << std::endl; err += (ori06.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 39 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 39 done. Current error " << err << std::endl; err += (ori07.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 40 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 40 done. Current error " << err << std::endl; err += (ori10.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 41 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 41 done. Current error " << err << std::endl; err += (ori11.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 42 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 42 done. Current error " << err << std::endl; err += (ori12.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 43 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 43 done. Current error " << err << std::endl; err += (ori13.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 44 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 44 done. Current error " << err << std::endl; err += (ori14.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 45 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 45 done. Current error " << err << std::endl; err += (ori15.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 46 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 46 done. Current error " << err << std::endl; err += (ori16.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 47 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 47 done. Current error " << err << std::endl; err += (ori17.toMatrix3() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 48 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 48 done. Current error " << err << std::endl; } { @@ -607,7 +757,7 @@ int testOrientation(int errcode) kine::Orientation ori3 = ori2.inverse() * ori1; err += (Quaternion(ori3.toVector4()).toRotationMatrix() - Matrix3::Identity()).norm(); - std::cout << "Multiplication test 49 done. Current error " << err << std::endl; + // std::cout << "Multiplication test 49 done. Current error " << err << std::endl; } { @@ -768,7 +918,7 @@ int testKinematics(int errcode) } } - std::cout << "Error 1 : " << err << std::endl; + std::cout << "Error 1 " << err << std::endl; if(err > threshold) { @@ -785,6 +935,13 @@ int testKinematics(int errcode) for(int i = 0; i < count; i++) { + /* + std::cout << std::endl << "-----------------------------------------" << std::endl + << "New iteration" + << std::endl << "-----------------------------------------" << std::endl; + std::cout << "err: " << err << std::endl; + */ + Vector3 pos0 = tools::ProbabilityLawSimulation::getUniformMatrix(); kine::Orientation ori0 = kine::Orientation::randomRotation(); Vector3 linvel0 = tools::ProbabilityLawSimulation::getUniformMatrix(); @@ -803,7 +960,9 @@ int testKinematics(int errcode) l = k; double dt = 0.001; + // std::cout << std::endl << "k before integrate: " << std::endl << k << std::endl; k.integrate(dt); + // std::cout << std::endl << "k after integrate: " << std::endl << k << std::endl; k0.reset(); k1.reset(); @@ -812,7 +971,7 @@ int testKinematics(int errcode) { k0.position = l.position; } - if(flag0 & Flags::orientation) + if(true) { k0.orientation = l.orientation; } @@ -891,7 +1050,10 @@ int testKinematics(int errcode) flag = flag | kine::Kinematics::Flags::angAcc; } + // << std::endl << "k0 before update: " << std::endl << k0 << std::endl; + // std::cout << std::endl << "k1 before update: " << std::endl << k1 << std::endl; k0.update(k1, dt, flag); + // std::cout << std::endl << "k0 after update: " << std::endl << k0 << std::endl; if(k0.position.isSet()) { @@ -954,7 +1116,7 @@ int testKinematics(int errcode) } } - std::cout << "Error 2 : " << err << std::endl; + std::cout << "Error 2 " << err << std::endl; if(err > threshold) { @@ -970,6 +1132,16 @@ int main() int returnVal; int errorcode = 0; + if((returnVal = testSetToDiffNoAliasKinematics(++errorcode))) + { + std::cout << "testSetToDiffNoAliasLocalKinematics Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testSetToDiffNoAliasLocalKinematics succeeded" << std::endl; + } + if((returnVal = testRotationOperations(++errorcode))) { std::cout << "testRotationOperations Failed, error code: " << returnVal << std::endl; @@ -1000,6 +1172,6 @@ int main() std::cout << "Kinematics test succeeded" << std::endl; } - std::cout << "test succeeded" << std::endl; + std::cout << "Every tests succeeded" << std::endl; return 0; } \ No newline at end of file diff --git a/unit-testings/test-kinetics-observer.cpp b/unit-testings/test-kinetics-observer.cpp index ed193f34..467c3cd2 100644 --- a/unit-testings/test-kinetics-observer.cpp +++ b/unit-testings/test-kinetics-observer.cpp @@ -21,7 +21,7 @@ int testKineticsObserverCodeAccessor(int errorcode) o.setSamplingTime(dt); - Kinematics stateKine; + LocalKinematics stateKine; stateKine.position.set() << 0.1, 0, 0.7; stateKine.orientation = Vector3(0, 0, 0); @@ -30,7 +30,7 @@ int testKineticsObserverCodeAccessor(int errorcode) o.setStateVector(x0); - o.setStateKinematics(stateKine); + o.setWorldCentroidStateKinematics(stateKine); o.setGyroBias(Vector3(1, 2, 3)); Vector6 wrench; @@ -45,9 +45,7 @@ int testKineticsObserverCodeAccessor(int errorcode) contactKine.position.set() << 0, 0.1, 0; contactKine.orientation.setZeroRotation(); - Vector6 initContactWrench = Vector6::Zero(); - - o.addContact(contactKine, initContactWrench, 0); + o.addContact(contactKine, 0); Matrix3 linStiffness, angStiffness, linDamping, angDamping; linStiffness.setZero(); @@ -63,7 +61,7 @@ int testKineticsObserverCodeAccessor(int errorcode) angDamping.diagonal().setConstant(20); contactKine.position.set() << 0, -0.1, 0; - o.addContact(contactKine, initContactWrench, 3, linStiffness, linDamping, angStiffness, angDamping); + o.addContact(contactKine, 3, linStiffness, linDamping, angStiffness, angDamping); Matrix12 initialCov, processCov; @@ -73,24 +71,23 @@ int testKineticsObserverCodeAccessor(int errorcode) processCov.diagonal().setConstant(0.0001); contactKine.position.set() << 1, 0.1, 0; - int i = o.addContact(contactKine, initContactWrench, initialCov, processCov); + int i = o.addContact(contactKine, initialCov, processCov); (void)i; /// avoid warning in release mode assert(i == 1); contactKine.position.set() << 1, -0.1, 0; - o.addContact(contactKine, initContactWrench, initialCov, processCov, 2, linDamping, linStiffness, angStiffness, - angDamping); + o.addContact(contactKine, initialCov, processCov, 2, linDamping, linStiffness, angStiffness, angDamping); std::cout << index << " " << x.transpose() << std::endl; o.update(); - Kinematics k = o.getKinematics(); + LocalKinematics k = o.getLocalCentroidKinematics(); std::cout << k; - Kinematics l = o.getKinematicsOf(k); + LocalKinematics l = o.getLocalKinematicsOf(k); std::cout << l; @@ -202,6 +199,7 @@ int testKineticsObserverCodeAccessor(int errorcode) int main() { + int returnVal; if((returnVal = testKineticsObserverCodeAccessor(3))) diff --git a/unit-testings/test-local-kinematics.cpp b/unit-testings/test-local-kinematics.cpp new file mode 100644 index 00000000..829f9530 --- /dev/null +++ b/unit-testings/test-local-kinematics.cpp @@ -0,0 +1,1521 @@ +#include +#include + +#include +#include + +using namespace stateObservation; +using namespace kine; + +/// @brief test rotationMatrix2Angle +/// +/// @param errorCode +/// @return int + +int testPositionByIntegration(int errcode) +{ + + double dt = 0.001; + int numbIterations = 10000; + + double err_pvwwd = 0; + double err_pwwd = 0; + double err_pw = 0; + double err_pwd = 0; + double err_pvw = 0; + double err_pvwd = 0; + double err_pavw = 0; + double err_pavwd = 0; + double err_pawwd = 0; + double err_paw = 0; + double err_pav = 0; + double err_pa = 0; + double err_pv = 0; + + double err2_pvwwd = 0; + double err2_pwwd = 0; + double err2_pw = 0; + double err2_pwd = 0; + double err2_pvw = 0; + double err2_pvwd = 0; + double err2_pavw = 0; + double err2_pavwd = 0; + double err2_pawwd = 0; + double err2_paw = 0; + double err2_pav = 0; + double err2_pa = 0; + double err2_pv = 0; + + for(int i = 0; i < numbIterations; i++) + { + LocalKinematics k; + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori = kine::Orientation::randomRotation(); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + k.position = pos; + k.orientation = ori; + k.linVel = linvel; + k.angVel = angvel; + k.linAcc = linacc; + k.angAcc = angacc; + + Vector3 p1_pavwwd = + k.position() - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position() - k.linVel())))) + + dt * (k.linVel() + 0.5 * dt * (k.linAcc() - k.angAcc().cross(k.position()))); + + Vector3 p1_pvwwd = + k.position() - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position() - k.linVel())))) + + dt * (k.linVel() + 0.5 * dt * (-k.angAcc().cross(k.position()))); + Vector3 p1_pwwd = k.position() - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position())))) + + dt * (0.5 * dt * (-k.angAcc().cross(k.position()))); + Vector3 p1_pw = k.position() - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position())))); + Vector3 p1_pwd = k.position() + dt * (0.5 * dt * (-k.angAcc().cross(k.position()))); + Vector3 p1_pvw = k.position() + - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position() - k.linVel())))) + + dt * (k.linVel()); + Vector3 p1_pvwd = k.position() + dt * (k.linVel() + 0.5 * dt * (-k.angAcc().cross(k.position()))); + Vector3 p1_pavw = k.position() + - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position() - k.linVel())))) + + dt * (k.linVel() + 0.5 * dt * (k.linAcc())); + Vector3 p1_pavwd = k.position() + dt * (k.linVel() + 0.5 * dt * (k.linAcc() - k.angAcc().cross(k.position()))); + Vector3 p1_pawwd = k.position() + - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position())))) + + dt * (0.5 * dt * (k.linAcc() - k.angAcc().cross(k.position()))); + Vector3 p1_paw = k.position() - k.angVel().cross(dt * (k.position() + dt * (0.5 * k.angVel().cross(k.position())))) + + dt * (0.5 * dt * (k.linAcc())); + Vector3 p1_pav = k.position() + dt * (k.linVel() + 0.5 * dt * (k.linAcc())); + Vector3 p1_pa = k.position() + dt * (0.5 * dt * (k.linAcc())); + Vector3 p1_pv = k.position() + dt * (k.linVel()); + + err_pvwwd += (p1_pavwwd - p1_pvwwd).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pwwd += (p1_pavwwd - p1_pwwd).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pw += (p1_pavwwd - p1_pw).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pwd += (p1_pavwwd - p1_pwd).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pvw += (p1_pavwwd - p1_pvw).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pvwd += (p1_pavwwd - p1_pvwd).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pavw += (p1_pavwwd - p1_pavw).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pavwd += (p1_pavwwd - p1_pavwd).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pawwd += (p1_pavwwd - p1_pawwd).squaredNorm() / p1_pavwwd.squaredNorm(); + err_paw += (p1_pavwwd - p1_paw).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pav += (p1_pavwwd - p1_pav).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pa += (p1_pavwwd - p1_pa).squaredNorm() / p1_pavwwd.squaredNorm(); + err_pv += (p1_pavwwd - p1_pv).squaredNorm() / p1_pavwwd.squaredNorm(); + + err2_pvwwd += (p1_pavwwd - p1_pvwwd)(0); + err2_pwwd += (p1_pavwwd - p1_pwwd)(0); + err2_pw += (p1_pavwwd - p1_pw)(0); + err2_pwd += (p1_pavwwd - p1_pwd)(0); + err2_pvw += (p1_pavwwd - p1_pvw)(0); + err2_pvwd += (p1_pavwwd - p1_pvwd)(0); + err2_pavw += (p1_pavwwd - p1_pavw)(0); + err2_pavwd += (p1_pavwwd - p1_pavwd)(0); + err2_pawwd += (p1_pavwwd - p1_pawwd)(0); + err2_paw += (p1_pavwwd - p1_paw)(0); + err2_pav += (p1_pavwwd - p1_pav)(0); + err2_pa += (p1_pavwwd - p1_pa)(0); + err2_pv += (p1_pavwwd - p1_pv)(0); + } + + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pvwwd" + << " with the full integration expression: " << err_pvwwd / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pwwd" + << " with the full integration expression: " << err_pwwd / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pw" + << " with the full integration expression: " << err_pw / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pwd" + << " with the full integration expression: " << err_pwd / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pvw" + << " with the full integration expression: " << err_pvw / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pvwd" + << " with the full integration expression: " << err_pvwd / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pavw" + << " with the full integration expression: " << err_pavw / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pavwd" + << " with the full integration expression: " << err_pavwd / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pawwd" + << " with the full integration expression: " << err_pawwd / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "paw" + << " with the full integration expression: " << err_paw / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pav" + << " with the full integration expression: " << err_pav / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pa" + << " with the full integration expression: " << err_pa / numbIterations << std::endl; + std::cout << std::endl + << "Average relative squared difference with the parameters " + << "pv" + << " with the full integration expression: " << err_pv / numbIterations << std::endl; + + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pvwwd" + << " with the full integration expression: " << err2_pvwwd / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pwwd" + << " with the full integration expression: " << err2_pwwd / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pw" + << " with the full integration expression: " << err2_pw / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pwd" + << " with the full integration expression: " << err2_pwd / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pvw" + << " with the full integration expression: " << err2_pvw / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pvwd" + << " with the full integration expression: " << err2_pvwd / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pavw" + << " with the full integration expression: " << err2_pavw / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pavwd" + << " with the full integration expression: " << err2_pavwd / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pawwd" + << " with the full integration expression: " << err2_pawwd / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "paw" + << " with the full integration expression: " << err2_paw / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pav" + << " with the full integration expression: " << err2_pav / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pa" + << " with the full integration expression: " << err2_pa / numbIterations << std::endl; + std::cout << std::endl + << "Average difference on the x direction with the parameters " + << "pv" + << " with the full integration expression: " << err2_pv / numbIterations << std::endl; + + return 0; +} + +int testSetToDiffNoAliasLocalKinematics(int errcode) +{ + + std::cout << "testSetToDiffNoAliasLocalKinematics test started" << std::endl; + typedef kine::LocalKinematics::Flags Flags; + + kine::LocalKinematics k0; + kine::LocalKinematics k1; + + Flags::Byte flag0 = BOOST_BINARY(000000); + Flags::Byte flag1 = BOOST_BINARY(000000); + + kine::LocalKinematics k, l, k2; + + int count = int(pow(2, 6) * pow(2, 6)); + double err = 0; + double threshold = 1e-30 * count; + + for(int i = 0; i < count; i++) + { + /* + std::cout << std::endl << "-----------------------------------------" << std::endl + << "New iteration" + << std::endl << "-----------------------------------------" << std::endl; + std::cout << "err: " << err << std::endl; + */ + + Vector3 pos0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori0 = kine::Orientation::randomRotation(); + Vector3 linvel0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + Vector3 pos1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori1 = kine::Orientation::randomRotation(); + Vector3 linvel1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel1 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc1 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + k0.reset(); + k1.reset(); + + if(true) /// the position has to be set + { + k0.position = pos0; + } + if(true) /// the orientation has to be set + { + k0.orientation = ori0; + } + if(flag0 & Flags::linVel) + { + k0.linVel = linvel0; + } + if(flag0 & Flags::angVel) + { + k0.angVel = angvel0; + } + if(flag0 & Flags::linAcc) + { + k0.linAcc = linacc0; + } + if(flag0 & Flags::angAcc) + { + k0.angAcc = angacc0; + } + + if(true) /// the position has to be set + { + k1.position = pos1; + } + if(true) /// the orientation has to be set + { + k1.orientation = ori1; + } + if(flag1 & Flags::linVel) + { + k1.linVel = linvel1; + } + if(flag1 & Flags::angVel) + { + k1.angVel = angvel1; + } + if(flag1 & Flags::linAcc) + { + k1.linAcc = linacc1; + } + if(flag1 & Flags::angAcc) + { + k1.angAcc = angacc1; + } + + if((flag0 = (flag0 + 1) & Flags::all) == 0) /// update the flags to span all the possibilties + flag1 = (flag1 + 1) & Flags::all; + + k2 = k1; + if(!k1.orientation.isSet()) + { + k2.orientation.setZeroRotation(); + } + LocalKinematics k3; + LocalKinematics k4; + + k3.setToDiffNoAlias(k2, k0); + + k4.setToProductNoAlias(k2, k0.getInverse()); + + // std::cout << std::endl << "k3: " << std::endl << k3 << std::endl; + // std::cout << std::endl << "k4: " << std::endl << k4 << std::endl; + + k = k4 * k3.getInverse(); + + if(k.position.isSet()) + { + err += k.position().squaredNorm(); + } + if(k.orientation.isSet()) + { + err += k.orientation.toRotationVector().squaredNorm(); + } + if(k.linVel.isSet()) + { + err += k.linVel().squaredNorm(); + } + if(k.angVel.isSet()) + { + err += k.angVel().squaredNorm(); + } + if(k.linAcc.isSet()) + { + err += k.linAcc().squaredNorm(); + } + if(k.angAcc.isSet()) + { + err += k.angAcc().squaredNorm(); + } + } + + std::cout << "Error 1 : " << err << std::endl; + + if(err > threshold) + { + std::cout << "Error too large : " << err << std::endl; + return errcode; + } + return 0; +} + +int testLocalVsGlobalIntegrates(int errcode) +{ + double threshold = 1e-4; + double err = 0; + + double dt = 0.001; + + Kinematics k; + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori = kine::Orientation::randomRotation(); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + k.position = pos; + k.orientation = ori; + k.linVel = linvel; + k.angVel = angvel; + k.linAcc = linacc; + k.angAcc = angacc; + + LocalKinematics lk1(k); + k.integrate(dt); + lk1.integrate(dt); + Kinematics k1 = Kinematics(lk1); + + std::cout << std::endl << "k1 : " << std::endl << k1 << std::endl; + std::cout << std::endl << "k : " << std::endl << k << std::endl; + + Kinematics diff = k1 * k.getInverse(); + + std::cout << std::endl << "diff : " << std::endl << diff << std::endl; + + std::cout << std::endl << "diff : " << std::endl << diff << std::endl; + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + + std::cout << "Error between integrations with local and global frames: " << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int testRotationOperations(int errorCode) +{ + unsigned numberOfTests = 1000; + for(unsigned currentTest = 0; currentTest < numberOfTests; ++currentTest) + { + { /// test isRotation + + Matrix3 R = tools::ProbabilityLawSimulation::getUniformMatrix(); + double testPecision = cst::epsilon1 * 20; + if(isRotationMatrix(R, testPecision)) + { + std::cout << "Test number " << currentTest << "isRotationTest failed: false positive" << std::endl; + return errorCode; + } + R = randomRotationQuaternion().toRotationMatrix(); + if(!isRotationMatrix(R, testPecision)) + { + std::cout << R.isUnitary() << " " << R.topLeftCorner<2, 2>().determinant() << " " << R(2, 2) << " " + << R.bottomLeftCorner<2, 2>().determinant() << " " << R(0, 2) << " " + << R.topLeftCorner<2, 2>().determinant() - R(2, 2) << " " + << R.bottomLeftCorner<2, 2>().determinant() - R(0, 2) << " " << testPecision << std::endl; + std::cout << "Test number " << currentTest << "isRotationTest failed: false negative" << std::endl; + return errorCode; + } + Matrix3 R2; + R2 << R.col(1), R.col(0), R.col(2); /// Non right handed orthogonal matrix + if(isRotationMatrix(R2, testPecision)) + { + std::cout << "isRotationTest failed: false positive (right-handedness)" << std::endl; + return errorCode; + } + } + { + /// test pure yaw + if(!isPureYaw(AngleAxis(randomAngle(), Vector3::UnitZ()).matrix()) + || isPureYaw(randomRotationQuaternion().toRotationMatrix())) + { + std::cout << "Test number " << currentTest << "Pure yaw detection failed " << std::endl; + return errorCode; + } + } + { + /// Test the angle made by one vector by a rotation + Vector3 axis = tools::ProbabilityLawSimulation::getGaussianMatrix().normalized(); + Vector3 v = tools::ProbabilityLawSimulation::getGaussianMatrix().cross(axis).normalized(); + double angle = randomAngle(); + Matrix3 m = (AngleAxis(angle, axis)).matrix() * AngleAxis(randomAngle(), v).matrix(); + + double error = fabs(angle - kine::rotationMatrixToAngle(m, axis, v)); + + if(error > cst::epsilon1 * 2 * M_PI) + { + std::cout << "Test number " << currentTest << "Test vector angle failed. Angle error " << error << std::endl; + return errorCode; + } + } + { + /// Test yaw extraction with custom vector + double angle = randomAngle(); + + Vector2 v = tools::ProbabilityLawSimulation::getGaussianMatrix().normalized(); + Vector3 v3; + v3 << v, 0; + Matrix3 m = AngleAxis(angle, Vector3::UnitZ()).matrix() * AngleAxis(randomAngle(), v3).matrix(); + + double error = fabs(angle - kine::rotationMatrixToYaw(m, v)); + + if(error > cst::epsilon1 * 2 * M_PI) + { + std::cout << "Test number " << currentTest << "Test Yaw extraction with custom vector failed. Angle error " + << error << std::endl; + return errorCode; + } + } + { + /// Test yaw extraction from roll pitch yaw using the x axis alignment + double rollangle = randomAngle(); + double pitchangle = randomAngle() / 2; /// constrain the pitch to be smaller than pi/2 + double yawangle = randomAngle(); + + Matrix3 m = rollPitchYawToRotationMatrix(rollangle, pitchangle, yawangle); + + double error = fabs(yawangle - kine::rotationMatrixToYaw(m)); + + if(error > cst::epsilon1 * 1e5 * M_PI) /// this function is really not precise + { + std::cout << "Test number " << currentTest << " axis-based failed. Angle" << yawangle << " Angle error " + << error << std::endl; + return errorCode; + } + + /// Test yaw extraction from roll pitch yaw using the traditional conversion + Vector3 rpy = kine::rotationMatrixToRollPitchYaw(m); + error = fabs(yawangle - rpy(2)); + + if(error > cst::epsilon1 * 1e5 * M_PI) /// this function is really not precise + { + std::cout << "Test number " << currentTest << "eigen-based failed. Angle" << yawangle << " Angle error " + << error << std::endl; + return errorCode; + } + } + + { + /// Test the automatic detection of the vector to use to extract yaw from a rotation matrix + double angle = randomAngle(); /// random value + Vector2 v = tools::ProbabilityLawSimulation::getGaussianMatrix().normalized(); + Vector3 v3; + v3 << v, 0; + + Matrix3 m = AngleAxis(angle, Vector3::UnitZ()).matrix() * AngleAxis(randomAngle(), v3).matrix(); + + double error = fabs(angle - kine::rotationMatrixToYawAxisAgnostic(m)); + + if(error > cst::epsilon1 * 2 * M_PI) + { + std::cout << " Test rotationMatrixToYawAxisAgnostic failed. Angle error " << error << std::endl; + + return errorCode; + } + } + { + Vector3 horizAxis1; + horizAxis1(2) = 0; + horizAxis1.head<2>() = tools::ProbabilityLawSimulation::getGaussianMatrix().normalized(); + double tiltAngle1 = randomAngle(); + double yawAngle = randomAngle(); + + Matrix3 yaw = AngleAxis(yawAngle, Vector3::UnitZ()).matrix(); + + /// we should get back this matrix + Matrix3 initialMatrix = yaw * AngleAxis(tiltAngle1, horizAxis1).matrix(); + + Vector3 tilt = initialMatrix.transpose() * Vector3::UnitZ(); + + /// m sis a random horizontal vector + Vector3 m; + m(2) = 0; + m.head<2>() = tools::ProbabilityLawSimulation::getGaussianMatrix().normalized(); + + Vector3 ml = initialMatrix.transpose() * m; + + Vector3 newTilt = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Matrix3 mTemp1, mTemp2; + + mTemp1 << m, m.cross(Vector3::UnitZ().cross(m)).normalized(), m.cross(Vector3::UnitZ()).normalized(); + + mTemp2 << ml, ml.cross(newTilt.cross(ml)).normalized(), ml.cross(newTilt).normalized(); + + /// This is a matrix such that M2.transpose()*m is orthogonal to tilt + Matrix3 M2 = mTemp1 * mTemp2.transpose(); + + Matrix3 estimatedMatrix = kine::mergeTiltWithYawAxisAgnostic(tilt, M2); + + if(!isRotationMatrix(estimatedMatrix, cst::epsilon1 * 10)) + { + std::cout << "Test mergeTiltWithYawAxisAgnostic failed. Reconstructed matrix is not a Rotation Matrix" + << std::endl; + return errorCode; + } + + double error = AngleAxis(initialMatrix * estimatedMatrix.transpose()).angle(); + + if(error > cst::epsilon1 * 1e5 * M_PI) /// this function is really not precise + { + std::cout << "Test mergeTiltWithYawAxisAgnostic failed. Reconstructed matrix is wrong" << std::endl; + return errorCode; + } + } + } /// end of for + return 0; +} + +int testOrientation(int errcode) +{ + + Vector4 q_i; + + typedef tools::ProbabilityLawSimulation ran; + + /// random orientation + q_i = ran::getGaussianMatrix(); + q_i.normalize(); + + /// several representations of the orientation + Quaternion q(q_i); + AngleAxis aa(q); + Matrix3 M(q.toRotationMatrix()); + + double err = 0.; + + std::cout << "Orientation test started " << err << std::endl; + + { + kine::Orientation ori1; + ori1 = q; + Matrix3 M1 = ori1; + + err += + (Quaternion(ori1.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + (M1 - M).norm(); + std::cout << "Assignment operaton test 1 (Quaternion) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori2; + ori2 = M; + + err += (Quaternion(ori2.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + + (ori2.toMatrix3() - M).norm(); + std::cout << "Assignment operaton test 2 (Matrix3) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori3; + ori3 = aa; + + err += (Quaternion(ori3.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + + (ori3.toMatrix3() - M).norm(); + std::cout << "Assignment operaton test 3 (AngleAxis) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori4; + ori4 = Vector3(aa.angle() * aa.axis()); + + err += (Quaternion(ori4.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + + (ori4.toMatrix3() - M).norm(); + std::cout << "Assignment operaton test 3 (Vector3) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori1(q); + Matrix3 M1 = ori1; + + err += + (Quaternion(ori1.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + (M1 - M).norm(); + std::cout << "Cast operaton test 1 (Matrix3) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori2(M); + + err += (Quaternion(ori2.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + + (ori2.toMatrix3() - M).norm(); + std::cout << "copy constructor test 1 (Matrix3) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori3(aa); + + err += (Quaternion(ori3.toVector4()).toRotationMatrix() - Quaternion(q_i).toRotationMatrix()).norm() + + (ori3.toMatrix3() - M).norm(); + std::cout << "copy constructor test 2 (AngleAxis) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori4(Vector3(aa.angle() * aa.axis())); + + err += (Quaternion(ori4.toVector4()).toRotationMatrix() - M).norm() + (ori4.toMatrix3() - M).norm(); + std::cout << "copy constructor test 3 (AngleAxis) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori4(q, M); + + err += (Quaternion(ori4.toVector4()).toRotationMatrix() - M).norm() + (ori4.toMatrix3() - M).norm(); + std::cout << "Constructor test 1 (Quaternion, Matrix3) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori4; + ori4.setRandom(); + ori4 = M; + + err += (Quaternion(ori4.toVector4()).toRotationMatrix() - M).norm() + (ori4.toMatrix3() - M).norm(); + std::cout << "Update Assignement operator test 1 (Matrix3) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori4; + ori4.setRandom(); + ori4 = q; + + err += (Quaternion(ori4.toVector4()).toRotationMatrix() - M).norm() + (ori4.toMatrix3() - M).norm(); + std::cout << "Update Assignement operator test 2 (Quaternion) done. Current error " << err << std::endl; + } + + { + kine::Orientation ori1(q); + + kine::Orientation ori2 = ori1.inverse(); + kine::Orientation ori3 = ori2.inverse(); + + Matrix3 M3 = ori1; + + err += (Quaternion(ori3.toVector4()).toRotationMatrix() - M).norm() + (M3 - M).norm(); + std::cout << "Inverse operator test 1 done. Current error " << err << std::endl; + } + + { + + kine::Orientation ori0(q); + kine::Orientation ori1(M); + ori1 = ori1.inverse(); + const kine::Orientation & ori2 = ori0; + const kine::Orientation & ori3 = ori1; + + kine::Orientation ori00 = ori0 * ori1; + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori01 = ori1 * ori0; + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori02 = ori0 * ori3; + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori03 = ori3 * ori0; + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori04 = ori2 * ori1; + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori05 = ori1 * ori2; + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori06 = ori2 * ori3; + kine::Orientation ori07 = ori3 * ori2; + + kine::Orientation ori10(ori0, ori1); + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori11(ori1, ori0); + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori12(ori0, ori3); + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori13(ori3, ori0); + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori14(ori2, ori1); + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori15(ori1, ori2); + ori0 = q; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori16(ori2, ori3); + kine::Orientation ori17(ori3, ori2); + + err += (ori00.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 1 done. Current error " << err << std::endl; + err += (ori01.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 2 done. Current error " << err << std::endl; + err += (ori02.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 3 done. Current error " << err << std::endl; + err += (ori03.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 4 done. Current error " << err << std::endl; + err += (ori04.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 5 done. Current error " << err << std::endl; + err += (ori05.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 6 done. Current error " << err << std::endl; + err += (ori06.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 7 done. Current error " << err << std::endl; + err += (ori07.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 8 done. Current error " << err << std::endl; + err += (ori10.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 9 done. Current error " << err << std::endl; + err += (ori11.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 10 done. Current error " << err << std::endl; + err += (ori12.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 11 done. Current error " << err << std::endl; + err += (ori13.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 12 done. Current error " << err << std::endl; + err += (ori14.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 13 done. Current error " << err << std::endl; + err += (ori15.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 14 done. Current error " << err << std::endl; + err += (ori16.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 15 done. Current error " << err << std::endl; + err += (ori17.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 16 done. Current error " << err << std::endl; + } + + { + + kine::Orientation ori0(q); + kine::Orientation ori1(q); + ori1 = ori1.inverse(); + const kine::Orientation & ori2 = ori0; + const kine::Orientation & ori3 = ori1; + + kine::Orientation ori00 = ori0 * ori1; + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori01 = ori1 * ori0; + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori02 = ori0 * ori3; + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori03 = ori3 * ori0; + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori04 = ori2 * ori1; + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori05 = ori1 * ori2; + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori06 = ori2 * ori3; + kine::Orientation ori07 = ori3 * ori2; + + kine::Orientation ori10(ori0, ori1); + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori11(ori1, ori0); + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori12(ori0, ori3); + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori13(ori3, ori0); + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori14(ori2, ori1); + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori15(ori1, ori2); + ori0 = q; + ori1 = q; + ori1 = ori1.inverse(); + kine::Orientation ori16(ori2, ori3); + kine::Orientation ori17(ori3, ori2); + + err += (ori00.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 17 done. Current error " << err << std::endl; + err += (ori01.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 18 done. Current error " << err << std::endl; + err += (ori02.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 19 done. Current error " << err << std::endl; + err += (ori03.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 20 done. Current error " << err << std::endl; + err += (ori04.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 21 done. Current error " << err << std::endl; + err += (ori05.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 22 done. Current error " << err << std::endl; + err += (ori06.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 23 done. Current error " << err << std::endl; + err += (ori07.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 24 done. Current error " << err << std::endl; + err += (ori10.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 25 done. Current error " << err << std::endl; + err += (ori11.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 26 done. Current error " << err << std::endl; + err += (ori12.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 27 done. Current error " << err << std::endl; + err += (ori13.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 28 done. Current error " << err << std::endl; + err += (ori14.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 29 done. Current error " << err << std::endl; + err += (ori15.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 30 done. Current error " << err << std::endl; + err += (ori16.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 31 done. Current error " << err << std::endl; + err += (ori17.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 32 done. Current error " << err << std::endl; + } + + { + + kine::Orientation ori0(M); + kine::Orientation ori1(M); + ori1 = ori1.inverse(); + const kine::Orientation & ori2 = ori0; + const kine::Orientation & ori3 = ori1; + + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori00 = ori0 * ori1; + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori01 = ori1 * ori0; + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori02 = ori0 * ori3; + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori03 = ori3 * ori0; + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori04 = ori2 * ori1; + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori05 = ori1 * ori2; + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori06 = ori2 * ori3; + kine::Orientation ori07 = ori3 * ori2; + + kine::Orientation ori10(ori0, ori1); + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori11(ori1, ori0); + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori12(ori0, ori3); + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori13(ori3, ori0); + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori14(ori2, ori1); + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori15(ori1, ori2); + ori0 = M; + ori1 = M; + ori1 = ori1.inverse(); + kine::Orientation ori16(ori2, ori3); + kine::Orientation ori17(ori3, ori2); + + err += (ori00.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 33 done. Current error " << err << std::endl; + err += (ori01.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 34 done. Current error " << err << std::endl; + err += (ori02.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 35 done. Current error " << err << std::endl; + err += (ori03.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 36 done. Current error " << err << std::endl; + err += (ori04.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 37 done. Current error " << err << std::endl; + err += (ori05.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 38 done. Current error " << err << std::endl; + err += (ori06.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 39 done. Current error " << err << std::endl; + err += (ori07.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 40 done. Current error " << err << std::endl; + err += (ori10.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 41 done. Current error " << err << std::endl; + err += (ori11.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 42 done. Current error " << err << std::endl; + err += (ori12.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 43 done. Current error " << err << std::endl; + err += (ori13.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 44 done. Current error " << err << std::endl; + err += (ori14.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 45 done. Current error " << err << std::endl; + err += (ori15.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 46 done. Current error " << err << std::endl; + err += (ori16.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 47 done. Current error " << err << std::endl; + err += (ori17.toMatrix3() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 48 done. Current error " << err << std::endl; + } + + { + kine::Orientation ori1(q); + kine::Orientation ori2(M); + + kine::Orientation ori3 = ori2.inverse() * ori1; + + err += (Quaternion(ori3.toVector4()).toRotationMatrix() - Matrix3::Identity()).norm(); + // std::cout << "Multiplication test 49 done. Current error " << err << std::endl; + } + + { + Vector3 v1 = tools::ProbabilityLawSimulation::getGaussianMatrix() * 0.1; + kine::Orientation ori1(M); + kine::Orientation ori2(ori1); + ori2.integrateRightSide(v1); + + Vector3 v2 = ori1.differentiateRightSide(ori2); + + err += (v1 - v2).norm(); + std::cout << "Integration/differentiation test 1 done. Current error " << err << std::endl; + } + + { + Vector3 v1; + kine::Orientation ori1; + kine::Orientation ori2; + + ori1.setRandom(); + ori2.setRandom(); + + v1 = ori2.differentiateRightSide(ori1); + kine::Orientation oris = ori2.integrateRightSide(v1); + + err += oris.differentiateRightSide(ori1).norm(); + std::cout << "Integration/differentiation test 2 done. Current error " << err << std::endl; + } + + if(err > 1e-13) + { + return errcode; + } + return 0; +} + +int testKinematics(int errcode) +{ + + std::cout << "LocalKinematics test started" << std::endl; + typedef kine::LocalKinematics::Flags Flags; + + kine::LocalKinematics k0; + kine::LocalKinematics k1; + + Flags::Byte flag0 = BOOST_BINARY(000000); + Flags::Byte flag1 = BOOST_BINARY(000000); + + kine::LocalKinematics k, l, k2; + + int count = int(pow(2, 6) * pow(2, 6)); + double err = 0; + double threshold = 1e-30 * count; + + for(int i = 0; i < count; i++) + { + Vector3 pos0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori0 = kine::Orientation::randomRotation(); + Vector3 linvel0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + Vector3 pos1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori1 = kine::Orientation::randomRotation(); + Vector3 linvel1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel1 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc1 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc1 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + k0.reset(); + k1.reset(); + + if(flag0 & Flags::position) + { + k0.position = pos0; + } + if(true) /// the orientation has to be set + { + k0.orientation = ori0; + } + if(flag0 & Flags::linVel) + { + k0.linVel = linvel0; + } + if(flag0 & Flags::angVel) + { + k0.angVel = angvel0; + } + if(flag0 & Flags::linAcc) + { + k0.linAcc = linacc0; + } + if(flag0 & Flags::angAcc) + { + k0.angAcc = angacc0; + } + + if(flag1 & Flags::position) + { + k1.position = pos1; + } + if(!k1.position.isSet() || flag1 & Flags::orientation) /// the orientation has to be set + { + k1.orientation = ori1; + } + if(flag1 & Flags::linVel) + { + k1.linVel = linvel1; + } + if(flag1 & Flags::angVel) + { + k1.angVel = angvel1; + } + if(flag1 & Flags::linAcc) + { + k1.linAcc = linacc1; + } + if(flag1 & Flags::angAcc) + { + k1.angAcc = angacc1; + } + + if((flag0 = (flag0 + 1) & Flags::all) == 0) /// update the flags to span all the possibilties + flag1 = (flag1 + 1) & Flags::all; + + k2 = k1; + if(!k1.orientation.isSet()) + { + k2.orientation.setZeroRotation(); + } + + k = ((k0 * k2) * k2.getInverse()) * k0.getInverse(); + + if(k.position.isSet()) + { + err += k.position().squaredNorm(); + } + if(k.orientation.isSet()) + { + err += k.orientation.toRotationVector().squaredNorm(); + } + if(k.linVel.isSet()) + { + err += k.linVel().squaredNorm(); + } + if(k.angVel.isSet()) + { + err += k.angVel().squaredNorm(); + } + if(k.linAcc.isSet()) + { + err += k.linAcc().squaredNorm(); + } + if(k.angAcc.isSet()) + { + err += k.angAcc().squaredNorm(); + } + } + + std::cout << "Error 1 : " << err << std::endl; + + if(err > threshold) + { + std::cout << "Error too large : " << err << std::endl; + return errcode; + } + + err = 0; + + threshold = 1e-19 * count; + // threshold = 1e-9; + + flag0 = BOOST_BINARY(000000); + flag1 = BOOST_BINARY(000000); + + for(int i = 0; i < count; i++) + { + /* + std::cout << std::endl << "-----------------------------------------" << std::endl + << "New iteration" + << std::endl << "-----------------------------------------" << std::endl; + std::cout << "err: " << err << std::endl; + */ + + Vector3 pos0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + kine::Orientation ori0 = kine::Orientation::randomRotation(); + Vector3 linvel0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angvel0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + Vector3 linacc0 = tools::ProbabilityLawSimulation::getUniformMatrix(); + Vector3 angacc0 = tools::ProbabilityLawSimulation::getGaussianMatrix(); + + k.reset(); + k.position = pos0; + k.orientation = ori0; + k.linVel = linvel0; + k.angVel = angvel0; + k.linAcc = linacc0; + k.angAcc = angacc0; + + l = k; + + double dt = 0.001; + // std::cout << std::endl << "k before integrate: " << std::endl << k << std::endl; + k.integrate(dt); + // std::cout << std::endl << "k after integrate: " << std::endl << k << std::endl; + + k0.reset(); + k1.reset(); + + if(flag0 & Flags::position) + { + k0.position = l.position; + } + if(true) + { + k0.orientation = l.orientation; + } + if(flag0 & Flags::linVel) + { + k0.linVel = l.linVel; + } + if(flag0 & Flags::angVel) + { + k0.angVel = l.angVel; + } + if(flag0 & Flags::linAcc) + { + k0.linAcc = l.linAcc; + } + if(flag0 & Flags::angAcc) + { + k0.angAcc = l.angAcc; + } + + if(flag1 & Flags::position) + { + k1.position = k.position; + } + if(flag1 & Flags::orientation) + { + k1.orientation = k.orientation; + } + if(flag1 & Flags::linVel) + { + k1.linVel = k.linVel; + } + if(flag1 & Flags::angVel) + { + k1.angVel = k.angVel; + } + if(flag1 & Flags::linAcc) + { + k1.linAcc = k.linAcc; + } + if(flag1 & Flags::angAcc) + { + k1.angAcc = k.angAcc; + } + + if((flag0 = (flag0 + 1) & Flags::all) == 0) /// update the flags to span all the possibilties + flag1 = (flag1 + 1) & Flags::all; + + kine::LocalKinematics::Flags::Byte flag = BOOST_BINARY(000000); + + if(k1.position.isSet() + || (k0.position.isSet() && k0.linVel.isSet() && k0.linAcc.isSet() && k0.angVel.isSet() && k0.angAcc.isSet())) + { + flag = flag | kine::Kinematics::Flags::position; + } + if(true) // the orientation has to be computed + { + if(!k1.orientation.isSet() || !(k0.orientation.isSet() && k0.angVel.isSet() && k0.angAcc.isSet())) + { + continue; // if the the orientation can't be computed, the update function has to prompt an error, so we musn't + // test this combination + } + flag = flag | kine::LocalKinematics::Flags::orientation; + } + if(k1.linVel.isSet() || (k0.position.isSet() && k1.position.isSet() && k1.angVel.isSet()) + || (k0.linAcc.isSet() && k0.linVel.isSet() && k0.angVel.isSet())) + { + flag = flag | kine::Kinematics::Flags::linVel; + } + if(k1.angVel.isSet() || (k0.orientation.isSet() && k1.orientation.isSet()) + || (k0.angVel.isSet() && k0.angAcc.isSet())) + { + flag = flag | kine::Kinematics::Flags::angVel; + } + if(k1.linAcc.isSet() || (k0.linVel.isSet() && k1.linVel.isSet() && k1.angVel.isSet()) + || (k0.linVel.isSet() && k0.position.isSet() && k1.position.isSet() && k1.angAcc.isSet() && k1.angVel.isSet())) + { + flag = flag | kine::Kinematics::Flags::linAcc; + } + if(k1.angAcc.isSet() || (k0.angVel.isSet() && k1.angVel.isSet()) + || (k0.angVel.isSet() && k0.orientation.isSet() && k1.orientation.isSet())) + { + flag = flag | kine::Kinematics::Flags::angAcc; + } + + // std::cout << std::endl << "k0 before update: " << std::endl << k0 << std::endl; + // std::cout << std::endl << "k1 before update: " << std::endl << k1 << std::endl; + k0.update(k1, dt, flag); + // std::cout << std::endl << "k0 after update: " << std::endl << k0 << std::endl; + + // std::cout << "Error before all : " << err << std::endl; + if(k0.position.isSet()) + { + if((k.position() - k0.position()).squaredNorm() < 1e-13) + { + err += (k.position() - k0.position()).squaredNorm(); + } + else + { + err += (l.position() + - l.angVel().cross(dt * (l.position() + dt * (l.linVel() - 0.5 * l.angVel().cross(l.position())))) + + dt * (l.linVel() + 0.5 * dt * (l.linAcc() - l.angAcc().cross(l.position()))) - k0.position()) + .squaredNorm(); + } + // std::cout << "Error after pos : " << err << std::endl; + } + if(k0.orientation.isSet()) + { + err += (k0.orientation.differentiateRightSide(k.orientation)).squaredNorm(); + // std::cout << "Error after ori : " << err << std::endl; + } + if(k0.linVel.isSet()) + { + + if((k.linVel() - k0.linVel()).squaredNorm() < 1e-10) + { + err += (k.linVel() - k0.linVel()).squaredNorm(); + } + else if(((k.position() - l.position()) / dt + k.angVel().cross(k.position()) - k0.linVel()).squaredNorm() < 1e-10) + { + err += ((k.position() - l.position()) / dt + k.angVel().cross(k.position()) - k0.linVel()).squaredNorm(); + } + else + { + Vector3 tempAngVel = (l.orientation.differentiateRightSide(k.orientation) / dt); + err += ((k.position() - l.position()) / dt + tempAngVel.cross(k.position()) - k0.linVel()).squaredNorm(); + } + + // std::cout << "Error after linVel : " << err << std::endl; + } + if(k0.angVel.isSet()) + { + if((k.angVel() - k0.angVel()).squaredNorm() < 1e-10) + { + err += (k.angVel() - k0.angVel()).squaredNorm(); + } + else + { + err += (l.orientation.differentiateRightSide(k.orientation) / dt - k0.angVel()).squaredNorm(); + } + // std::cout << "Error after angVel : " << err << std::endl; + } + if(k0.linAcc.isSet()) + { + + if((k.linAcc() - k0.linAcc()).squaredNorm() < 1e-10) + { + err += (k.linAcc() - k0.linAcc()).squaredNorm(); + } + else if((k.linAcc() - 2 * k0.linAcc()).squaredNorm() < 1e-10) + { + err += (k.linAcc() - 2 * k0.linAcc()).squaredNorm(); + } + else if((k.angVel().cross(k.linVel()) + (k.linVel() - l.linVel()) / dt - k0.linAcc()).squaredNorm() < 1e-10) + { + err += (k.angVel().cross(k.linVel()) + (k.linVel() - l.linVel()) / dt - k0.linAcc()).squaredNorm(); + } + else + { + Vector3 tempLinVel = (k.position() - l.position()) / dt + k.angVel().cross(k.position()); + err += (k.angVel().cross(tempLinVel) + (tempLinVel - l.linVel()) / dt - k0.linAcc()).squaredNorm(); + } + + // std::cout << "Error after linAcc : " << err << std::endl; + } + if(k0.angAcc.isSet()) + { + if((k.angAcc() - k0.angAcc()).squaredNorm() < 1e-10) + { + err += (k.angAcc() - k0.angAcc()).squaredNorm(); + } + else + { + err += (k.angAcc() - 2 * k0.angAcc()).squaredNorm(); + } + + // std::cout << "Error after angAcc : " << err << std::endl; + } + + // std::cout<< i<<" "< threshold) + { + break; + } + } + + std::cout << "Error 2 : " << err << std::endl; + + if(err > threshold) + { + std::cout << "Error too large !" << std::endl; + return errcode; + } + + return 0; +} + +int main() +{ + int returnVal; + int errorcode = 0; + + if((returnVal = testPositionByIntegration(++errorcode))) + { + std::cout << "testPositionByIntegration Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testPositionByIntegration succeeded" << std::endl; + } + + if((returnVal = testSetToDiffNoAliasLocalKinematics(++errorcode))) + { + std::cout << "testSetToDiffNoAliasLocalKinematics Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testSetToDiffNoAliasLocalKinematics succeeded" << std::endl; + } + + if((returnVal = testLocalVsGlobalIntegrates(++errorcode))) + { + std::cout << "testLocalVsGlobalIntegrates Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testLocalVsGlobalIntegrates succeeded" << std::endl; + } + + if((returnVal = testRotationOperations(++errorcode))) + { + std::cout << "testRotationOperations Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testRotationOperations succeeded" << std::endl; + } + + if((returnVal = testOrientation(++errorcode))) /// it is not an equality check + { + std::cout << "Orientation test failed, code : 1" << std::endl; + return returnVal; + } + else + { + std::cout << "Orientation test succeeded" << std::endl; + } + + if((returnVal = testKinematics(++errorcode))) /// it is not an equality check + { + std::cout << "LocalKinematics test failed, code : 2" << std::endl; + return returnVal; + } + else + { + std::cout << "LocalKinematics test succeeded" << std::endl; + } + + std::cout << "test succeeded" << std::endl; + return 0; +} \ No newline at end of file diff --git a/unit-testings/test-runge-kutta.cpp b/unit-testings/test-runge-kutta.cpp new file mode 100644 index 00000000..fba5a759 --- /dev/null +++ b/unit-testings/test-runge-kutta.cpp @@ -0,0 +1,580 @@ +#include +#include + +#include +#include +#include + +using namespace stateObservation; +using namespace kine; + +/// @brief test rotationMatrix2Angle +/// +/// @param errorCode +/// @return int + +double lin_stiffness_ = (double)rand() / RAND_MAX * 1e5; +double lin_damping_ = (double)rand() / RAND_MAX * 5 * 1e1; +double ang_stiffness_ = (double)rand() / RAND_MAX * 1e5; +double ang_damping_ = (double)rand() / RAND_MAX * 5 * 1e1; + +Matrix3 K1 = lin_stiffness_ * Matrix3::Identity(); +Matrix3 K2 = lin_damping_ * Matrix3::Identity(); +Matrix3 K3 = ang_stiffness_ * Matrix3::Identity(); +Matrix3 K4 = ang_damping_ * Matrix3::Identity(); + +struct FunctorConstAccLocKine : kine::LocalKinematics::RecursiveAccelerationFunctorBase +{ + FunctorConstAccLocKine() {} + + void computeRecursiveLocalAccelerations_(LocalKinematics & locKine) + { + locKine.linAcc = linAcc; + locKine.angAcc = angAcc; + } + Vector3 linAcc; + Vector3 angAcc; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct FunctorVarAccLocKine : kine::LocalKinematics::RecursiveAccelerationFunctorBase +{ + FunctorVarAccLocKine() {} + + void computeRecursiveLocalAccelerations_(LocalKinematics & locKine) + { + Kinematics kine(locKine); + kine.linAcc = -K1 * locKine.position() - K2 * locKine.linVel(); + kine.angAcc = -K3 * locKine.orientation.toRotationVector() - K4 * locKine.angVel(); + locKine = kine; + } +}; + +struct FunctorConstAccKine : kine::Kinematics::RecursiveAccelerationFunctorBase +{ + FunctorConstAccKine() {} + + void computeRecursiveGlobalAccelerations_(Kinematics & kine) + { + kine.linAcc = linAcc; + kine.angAcc = angAcc; + } + Vector3 linAcc; + Vector3 angAcc; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct FunctorVarAccKine : kine::Kinematics::RecursiveAccelerationFunctorBase +{ + FunctorVarAccKine() {} + + void computeRecursiveGlobalAccelerations_(Kinematics & kine) + { + kine.linAcc = -K1 * kine.position() - K2 * kine.linVel(); + kine.angAcc = -K3 * kine.orientation.toRotationVector() - K4 * kine.angVel(); + } +}; + +struct FunctorConstAccKineSameAsLocKine : kine::Kinematics::RecursiveAccelerationFunctorBase +{ + FunctorConstAccKineSameAsLocKine() {} + + void computeRecursiveGlobalAccelerations_(Kinematics & kine) + { + kine.linAcc = kine.orientation * linAcc; // we consider the acceleration is constant in the local frame + kine.angAcc = kine.orientation * angAcc; + } + Vector3 linAcc; + Vector3 angAcc; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +int testRungeKuttaLocalKinematicsConstantAcceleration(int errcode) // 1 +{ + FunctorConstAccLocKine functorAccConst; + + functorAccConst.linAcc = tools::ProbabilityLawSimulation::getUniformMatrix(); + functorAccConst.angAcc = tools::ProbabilityLawSimulation::getUniformMatrix(); + + double dt = 0.005; + double threshold = 1e-4; + double err = 0; + + LocalKinematics k; + + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + kine::Orientation ori(Vector3(tools::ProbabilityLawSimulation::getUniformMatrix() / 10)); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; + + k.position = pos; + k.orientation = ori; + k.linVel = linvel; + k.angVel = angvel; + + functorAccConst.computeRecursiveLocalAccelerations_(k); + + LocalKinematics rk(k); + + rk.integrateRungeKutta4(dt, functorAccConst); + + int maxT = 10; + for(int i = 0; i < maxT; i++) + { + k.integrate(dt / maxT); + } + + LocalKinematics diff = rk * k.getInverse(); + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + + err = sqrt(err); + + std::cout << "Error between Runge Kutta integrations and consecutive integrations for LocalKinematics with constant " + "acceleration: " + << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int testRungeKuttaKinematicsConstantAcceleration(int errcode) // 2 +{ + FunctorConstAccKine functorAccConst; + + functorAccConst.linAcc = tools::ProbabilityLawSimulation::getUniformMatrix(); + functorAccConst.angAcc = tools::ProbabilityLawSimulation::getUniformMatrix(); + + double dt = 0.005; + double threshold = 1e-4; + double err = 0; + + Kinematics k; + + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + kine::Orientation ori(Vector3(tools::ProbabilityLawSimulation::getUniformMatrix() / 10)); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; + + k.position = pos; + k.orientation = ori; + k.linVel = linvel; + k.angVel = angvel; + + functorAccConst.computeRecursiveGlobalAccelerations_(k); + + Kinematics rk(k); + + rk.integrateRungeKutta4(dt, functorAccConst); + + int maxT = 10; + for(int i = 0; i < maxT; i++) + { + k.integrate(dt / maxT); + } + + Kinematics diff = rk * k.getInverse(); + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + err = sqrt(err); + + std::cout << "Error between Runge Kutta integrations and consecutive integrations for Kinematics with constant " + "acceleration: " + << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int testRungeKuttaLocalKinematicsVariableAcceleration(int errcode) // 3 +{ + FunctorVarAccLocKine functorVarAcc; + + lin_stiffness_ = (double)rand() / RAND_MAX * 1e1; + lin_damping_ = (double)rand() / RAND_MAX * 5 * 1e-2; + ang_stiffness_ = (double)rand() / RAND_MAX * 1e1; + ang_damping_ = (double)rand() / RAND_MAX * 5 * 1e0; + + K1 = lin_stiffness_ * Matrix3::Identity(); + K2 = lin_damping_ * Matrix3::Identity(); + K3 = ang_stiffness_ * Matrix3::Identity(); + K4 = ang_damping_ * Matrix3::Identity(); + + double dt = 0.005; + double threshold = 1e-4; + double err = 0; + + LocalKinematics k; + + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + kine::Orientation ori(Vector3(tools::ProbabilityLawSimulation::getUniformMatrix() / 10)); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; + + k.position = pos; + k.orientation = ori; + k.linVel = linvel; + k.angVel = angvel; + + functorVarAcc.computeRecursiveLocalAccelerations_(k); + + LocalKinematics rk(k); + + rk.integrateRungeKutta4(dt, functorVarAcc); + + int maxT = 10; + for(int i = 0; i < maxT; i++) + { + k.integrate(dt / maxT); + functorVarAcc.computeRecursiveLocalAccelerations_(k); + } + + LocalKinematics diff = rk * k.getInverse(); + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + err = sqrt(err); + + std::cout << "Error between Runge Kutta integrations and consecutive integrations for LocalKinematics with variable " + "acceleration: " + << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int testRungeKuttaKinematicsVariableAcceleration(int errcode) // 4 +{ + FunctorVarAccKine functorVarAcc; + + lin_stiffness_ = (double)rand() / RAND_MAX * 3e2; + lin_damping_ = (double)rand() / RAND_MAX * 5 * 5e-1; + ang_stiffness_ = (double)rand() / RAND_MAX * 3e2; + ang_damping_ = (double)rand() / RAND_MAX * 5 * 5e-2; + + K1 = lin_stiffness_ * Matrix3::Identity(); + K2 = lin_damping_ * Matrix3::Identity(); + K3 = ang_stiffness_ * Matrix3::Identity(); + K4 = ang_damping_ * Matrix3::Identity(); + + double dt = 0.005; + double threshold = 1e-4; + double err = 0; + + Kinematics k; + + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + kine::Orientation ori(Vector3(tools::ProbabilityLawSimulation::getUniformMatrix() / 10)); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; + + k.position = pos; + k.orientation = ori; + k.linVel = linvel; + k.angVel = angvel; + + functorVarAcc.computeRecursiveGlobalAccelerations_(k); + + Kinematics rk(k); + + rk.integrateRungeKutta4(dt, functorVarAcc); + + int maxT = 10; + for(int i = 0; i < maxT; i++) + { + k.integrate(dt / maxT); + functorVarAcc.computeRecursiveGlobalAccelerations_(k); + } + + Kinematics diff = rk * k.getInverse(); + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + err = sqrt(err); + + std::cout << "Error between Runge Kutta integrations and consecutive integrations for Kinematics with variable " + "acceleration: " + << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int testRungeKuttaKinematicsVsLocalKinematicsConstantAcceleration(int errcode) // 5 +{ + FunctorConstAccLocKine functorConstAccLocKine; + FunctorConstAccKineSameAsLocKine functorConstAcctKine; + + functorConstAccLocKine.linAcc = functorConstAcctKine.linAcc = + tools::ProbabilityLawSimulation::getUniformMatrix(); + functorConstAccLocKine.angAcc = functorConstAcctKine.angAcc = + tools::ProbabilityLawSimulation::getUniformMatrix(); + + double dt = 0.005; + double threshold = 1e-4; + double err = 0; + int iterations = 1; + + LocalKinematics lk; + + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + kine::Orientation ori(Vector3(tools::ProbabilityLawSimulation::getUniformMatrix() / 10)); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix() / 10; + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix() / 10; + + lk.position = pos; + lk.orientation = ori; + lk.linVel = linvel; + lk.angVel = angvel; + + Kinematics k(lk); + + functorConstAccLocKine.computeRecursiveLocalAccelerations_(lk); + functorConstAcctKine.computeRecursiveGlobalAccelerations_(k); + + for(int i = 0; i < iterations; i++) lk.integrateRungeKutta4(dt, functorConstAccLocKine); + k.integrateRungeKutta4(dt, functorConstAcctKine); + + Kinematics diff = Kinematics(lk) * k.getInverse(); + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + err = sqrt(err); + + std::cout << "Error between Runge Kutta integrations for Kinematics vs LocalKinematics with constant acceleration: " + << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int testRungeKuttaKinematicsVsLocalKinematicsVariableAcceleration(int errcode) // 6 +{ + FunctorVarAccKine functorVarAccKine; + FunctorVarAccLocKine functorVarAccLocKine; + + lin_stiffness_ = (double)rand() / RAND_MAX * 3e2; + lin_damping_ = (double)rand() / RAND_MAX * 5 * 5e-1; + ang_stiffness_ = (double)rand() / RAND_MAX * 3e2; + ang_damping_ = (double)rand() / RAND_MAX * 5 * 5e-2; + + K1 = lin_stiffness_ * Matrix3::Identity(); + K2 = lin_damping_ * Matrix3::Identity(); + K3 = ang_stiffness_ * Matrix3::Identity(); + K4 = ang_damping_ * Matrix3::Identity(); + + double dt = 0.005; + double threshold = 1e-4; + double err = 0; + int iterations = 1; + + LocalKinematics lk; + + Vector3 pos = tools::ProbabilityLawSimulation::getUniformMatrix() / 40; + kine::Orientation ori(Vector3(tools::ProbabilityLawSimulation::getUniformMatrix() / 25)); + Vector3 linvel = tools::ProbabilityLawSimulation::getUniformMatrix() / 40; + Vector3 angvel = tools::ProbabilityLawSimulation::getGaussianMatrix() / 25; + + lk.position = pos; + lk.orientation = ori; + lk.linVel = linvel; + lk.angVel = angvel; + + Kinematics k(lk); + + functorVarAccLocKine.computeRecursiveLocalAccelerations_(lk); + functorVarAccKine.computeRecursiveGlobalAccelerations_(k); + + for(int i = 0; i < iterations; i++) + { + lk.integrateRungeKutta4(dt, functorVarAccLocKine); + k.integrateRungeKutta4(dt, functorVarAccKine); + } + + Kinematics diff = Kinematics(lk) * k.getInverse(); + + if(diff.position.isSet()) + { + err += diff.position().squaredNorm(); + } + if(diff.orientation.isSet()) + { + err += diff.orientation.toRotationVector().squaredNorm(); + } + if(diff.linVel.isSet()) + { + err += diff.linVel().squaredNorm(); + } + if(diff.angVel.isSet()) + { + err += diff.angVel().squaredNorm(); + } + err = sqrt(err); + + std::cout << "Error between Runge Kutta integrations for Kinematics vs LocalKinematics with variable acceleration: " + << err << std::endl; + + if(err > threshold) + { + return errcode; + } + return 0; +} + +int main() +{ + int returnVal; + int errorcode = 0; + + if((returnVal = testRungeKuttaLocalKinematicsConstantAcceleration(++errorcode))) + { + std::cout << "testRungeKuttaLocalKinematicsConstantAcceleration Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testRungeKuttaLocalKinematicsConstantAcceleration succeeded" << std::endl; + } + + if((returnVal = testRungeKuttaKinematicsConstantAcceleration(++errorcode))) + { + std::cout << "testRungeKuttaKinematicsConstantAcceleration Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testRungeKuttaKinematicsConstantAcceleration succeeded" << std::endl; + } + + if((returnVal = testRungeKuttaLocalKinematicsVariableAcceleration(++errorcode))) + { + std::cout << "testRungeKuttaLocalKinematicsVariableAcceleration Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testRungeKuttaLocalKinematicsVariableAcceleration succeeded" << std::endl; + } + + if((returnVal = testRungeKuttaKinematicsVariableAcceleration(++errorcode))) + { + std::cout << "testRungeKuttaKinematicsVariableAcceleration Failed, error code: " << returnVal << std::endl; + return returnVal; + } + else + { + std::cout << "testRungeKuttaKinematicsVariableAcceleration succeeded" << std::endl; + } + + if((returnVal = testRungeKuttaKinematicsVsLocalKinematicsConstantAcceleration(++errorcode))) + { + std::cout << "testRungeKuttaKinematicsVsLocalKinematicsConstantAcceleration Failed, error code: " << returnVal + << std::endl; + return returnVal; + } + else + { + std::cout << "testRungeKuttaKinematicsVsLocalKinematicsConstantAcceleration succeeded" << std::endl; + } + + if((returnVal = testRungeKuttaKinematicsVsLocalKinematicsVariableAcceleration(++errorcode))) + { + std::cout << "testRungeKuttaKinematicsVsLocalKinematicsVariableAcceleration Failed, error code: " << returnVal + << std::endl; + return returnVal; + } + else + { + std::cout << "testRungeKuttaKinematicsVsLocalKinematicsVariableAcceleration succeeded" << std::endl; + } + + std::cout << "test succeeded" << std::endl; + return 0; +} \ No newline at end of file