diff --git a/.github/workflows/continuous_integration.yml b/.github/workflows/continuous_integration.yml index f8c222f5d7..cf15fce96a 100644 --- a/.github/workflows/continuous_integration.yml +++ b/.github/workflows/continuous_integration.yml @@ -15,18 +15,17 @@ on: - '*' jobs: - windows: - name: Windows - + windows2019: + name: Windows 2019 runs-on: windows-2019 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install Doxygen # choco install doxygen.portable # <-- too unreliable. run: | - (New-Object System.Net.WebClient).DownloadFile("https://sourceforge.net/projects/myosin/files/doxygen-1.8.14.windows.x64.bin.zip/download", "doxygen.zip") + (New-Object System.Net.WebClient).DownloadFile("https://github.com/doxygen/doxygen/releases/download/Release_1_12_0/doxygen-1.12.0.windows.x64.bin.zip", "doxygen.zip") 7z x $env:GITHUB_WORKSPACE/doxygen.zip -odoxygen echo "$env:GITHUB_WORKSPACE\\doxygen" >> $GITHUB_PATH @@ -115,58 +114,23 @@ jobs: - name: Upload opensim-core uses: actions/upload-artifact@v4 with: - name: opensim-core-${{ steps.configure.outputs.version }}-win-2019 + name: opensim-core-${{ steps.configure.outputs.version }}-win2019 path: opensim-core-${{ steps.configure.outputs.version }}.zip - # TODO: Must be relative to the workspace? - # - name: Upload opensim-moco dependencies - # uses: actions/upload-artifact@v1.0.0 - # with: - # name: opensim-core-dependencies - # path: opensim_dependencies_install.zip - - # - name: Create release - # id: create-release - # uses: actions/create-release@v1.0.0 - # env: - # GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - # with: - # tag_name: ${{ github.ref }} - # release_name: OpenSim Core ${{ github.ref }} - # draft: true - # prerelease: false - - # - name: Zip opensim-core - # run: | - # 7z a opensim-core-$VERSION.zip opensim-core-${VERSION} - - # - name: Upload release asset - # id: upload-release-asset - # uses: actions/upload-release-asset@v1.0.1 - # env: - # GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - # with: - # upload_url: ${{ steps.create-release.outputs.upload_url }} - # asset_path: ./opensim-core-${{ github.ref }}.zip - # asset_name: opensim-core-${{ github.ref }}.zip - # asset_content_type: application/zip - - windows2022_target: - name: Windows2022 [target] - - if: ${{ contains(github.event.pull_request.title, '[perf-win]') || contains(github.event.pull_request.body, '[perf-win]') }} - + windows2022: runs-on: windows-2022 + name: Windows 2022 + + outputs: + version: ${{ steps.configure.outputs.version }} steps: - - uses: actions/checkout@v3 - with: - ref: ${{ github.event.pull_request.base.ref }} + - uses: actions/checkout@v4 - name: Install Doxygen # choco install doxygen.portable # <-- too unreliable. run: | - (New-Object System.Net.WebClient).DownloadFile("https://sourceforge.net/projects/myosin/files/doxygen-1.8.14.windows.x64.bin.zip/download", "doxygen.zip") + (New-Object System.Net.WebClient).DownloadFile("https://github.com/doxygen/doxygen/releases/download/Release_1_12_0/doxygen-1.12.0.windows.x64.bin.zip", "doxygen.zip") 7z x $env:GITHUB_WORKSPACE/doxygen.zip -odoxygen echo "$env:GITHUB_WORKSPACE\\doxygen" >> $GITHUB_PATH @@ -174,7 +138,7 @@ jobs: uses: actions/setup-python@v4 with: python-version: '3.10' - + - name: Install numpy #Need numpy to use SWIG numpy typemaps. run: python3 -m pip install numpy==1.25 @@ -217,7 +181,7 @@ jobs: # TODO: Can remove /WX when we use that in CMakeLists.txt. # Set the CXXFLAGS environment variable to turn warnings into errors. # Skip timing test section included by default. - cmake -E env CXXFLAGS="/WX -DSKIP_TIMING" cmake $env:GITHUB_WORKSPACE -LAH -G"Visual Studio 17 2022" -A x64 -DCMAKE_INSTALL_PREFIX=~/opensim-core-install -DOPENSIM_DEPENDENCIES_DIR=~/opensim_dependencies_install -DOPENSIM_C3D_PARSER=ezc3d -DBUILD_PYTHON_WRAPPING=on -DBUILD_JAVA_WRAPPING=on -DPython3_ROOT_DIR=C:\hostedtoolcache\windows\Python\3.10.11\x64 + cmake -E env CXXFLAGS="/WX -DSKIP_TIMING" cmake $env:GITHUB_WORKSPACE -LAH -G"Visual Studio 17 2022" -A x64 -DCMAKE_INSTALL_PREFIX=~/opensim-core-install -DOPENSIM_DEPENDENCIES_DIR=~/opensim_dependencies_install -DOPENSIM_C3D_PARSER=ezc3d -DOPENSIM_WITH_TROPTER=on -DOPENSIM_WITH_CASADI=on -DBUILD_PYTHON_WRAPPING=on -DBUILD_JAVA_WRAPPING=on -DPython3_ROOT_DIR=C:\hostedtoolcache\windows\Python\3.10.11\x64 $env:match = cmake -L . | Select-String -Pattern OPENSIM_QUALIFIED_VERSION $version = $env:match.split('=')[1] echo $version @@ -242,8 +206,8 @@ jobs: run: | chdir $env:GITHUB_WORKSPACE\\build chdir $env:GITHUB_WORKSPACE - Copy-Item -Path "~/opensim-core-install" -Destination "opensim-core-${{ github.event.pull_request.base.ref }}" -Recurse - 7z a "opensim-core-${{ github.event.pull_request.base.ref }}.zip" "opensim-core-${{ github.event.pull_request.base.ref }}" + Copy-Item -Path "~/opensim-core-install" -Destination "opensim-core-${{ steps.configure.outputs.version }}" -Recurse + 7z a "opensim-core-${{ steps.configure.outputs.version }}.zip" "opensim-core-${{ steps.configure.outputs.version }}" - name: Test Python bindings run: | @@ -256,25 +220,24 @@ jobs: - name: Upload opensim-core uses: actions/upload-artifact@v4 with: - name: opensim-core-${{ github.event.pull_request.base.ref }}-win - path: opensim-core-${{ github.event.pull_request.base.ref }}.zip + name: opensim-core-${{ steps.configure.outputs.version }}-win2022 + path: opensim-core-${{ steps.configure.outputs.version }}.zip - windows2022_source: - + windows2022_perf: runs-on: windows-2022 - - name: ${{ (contains(github.event.pull_request.title, '[perf-win]') || contains(github.event.pull_request.body, '[perf-win]')) && 'Windows2022 [source]' || 'Windows2022' }} + name: Windows 2022 [main] - outputs: - version: ${{ steps.configure.outputs.version }} + if: ${{ contains(github.event.pull_request.title, '[perf-win]') || contains(github.event.pull_request.body, '[perf-win]') }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.base.ref }} - name: Install Doxygen # choco install doxygen.portable # <-- too unreliable. run: | - (New-Object System.Net.WebClient).DownloadFile("https://sourceforge.net/projects/myosin/files/doxygen-1.8.14.windows.x64.bin.zip/download", "doxygen.zip") + (New-Object System.Net.WebClient).DownloadFile("https://github.com/doxygen/doxygen/releases/download/Release_1_12_0/doxygen-1.12.0.windows.x64.bin.zip", "doxygen.zip") 7z x $env:GITHUB_WORKSPACE/doxygen.zip -odoxygen echo "$env:GITHUB_WORKSPACE\\doxygen" >> $GITHUB_PATH @@ -282,7 +245,7 @@ jobs: uses: actions/setup-python@v4 with: python-version: '3.10' - + - name: Install numpy #Need numpy to use SWIG numpy typemaps. run: python3 -m pip install numpy==1.25 @@ -325,7 +288,7 @@ jobs: # TODO: Can remove /WX when we use that in CMakeLists.txt. # Set the CXXFLAGS environment variable to turn warnings into errors. # Skip timing test section included by default. - cmake -E env CXXFLAGS="/WX -DSKIP_TIMING" cmake $env:GITHUB_WORKSPACE -LAH -G"Visual Studio 17 2022" -A x64 -DCMAKE_INSTALL_PREFIX=~/opensim-core-install -DOPENSIM_DEPENDENCIES_DIR=~/opensim_dependencies_install -DOPENSIM_C3D_PARSER=ezc3d -DOPENSIM_WITH_TROPTER=on -DOPENSIM_WITH_CASADI=on -DBUILD_PYTHON_WRAPPING=on -DBUILD_JAVA_WRAPPING=on -DPython3_ROOT_DIR=C:\hostedtoolcache\windows\Python\3.10.11\x64 + cmake -E env CXXFLAGS="/WX -DSKIP_TIMING" cmake $env:GITHUB_WORKSPACE -LAH -G"Visual Studio 17 2022" -A x64 -DCMAKE_INSTALL_PREFIX=~/opensim-core-install -DOPENSIM_DEPENDENCIES_DIR=~/opensim_dependencies_install -DOPENSIM_C3D_PARSER=ezc3d -DBUILD_PYTHON_WRAPPING=on -DBUILD_JAVA_WRAPPING=on -DPython3_ROOT_DIR=C:\hostedtoolcache\windows\Python\3.10.11\x64 $env:match = cmake -L . | Select-String -Pattern OPENSIM_QUALIFIED_VERSION $version = $env:match.split('=')[1] echo $version @@ -350,8 +313,8 @@ jobs: run: | chdir $env:GITHUB_WORKSPACE\\build chdir $env:GITHUB_WORKSPACE - Copy-Item -Path "~/opensim-core-install" -Destination "opensim-core-${{ steps.configure.outputs.version }}" -Recurse - 7z a "opensim-core-${{ steps.configure.outputs.version }}.zip" "opensim-core-${{ steps.configure.outputs.version }}" + Copy-Item -Path "~/opensim-core-install" -Destination "opensim-core-${{ github.event.pull_request.base.ref }}" -Recurse + 7z a "opensim-core-${{ github.event.pull_request.base.ref }}.zip" "opensim-core-${{ github.event.pull_request.base.ref }}" - name: Test Python bindings run: | @@ -364,26 +327,106 @@ jobs: - name: Upload opensim-core uses: actions/upload-artifact@v4 with: - name: opensim-core-${{ steps.configure.outputs.version }}-win - path: opensim-core-${{ steps.configure.outputs.version }}.zip + name: opensim-core-${{ github.event.pull_request.base.ref }}-win2022 + path: opensim-core-${{ github.event.pull_request.base.ref }}.zip - mac_target: - name: Mac [target] + performance_analysis_win: + runs-on: ubuntu-latest + name: Performance Analysis [Windows 2022] - runs-on: macos-12 + env: + GH_TOKEN: ${{ github.token }} - if: ${{ contains(github.event.pull_request.title, '[perf-mac]') || contains(github.event.pull_request.body, '[perf-mac]') }} + needs: + - windows2022 + - windows2022_perf + + if: ${{ contains(github.event.pull_request.title, '[perf-win]') || contains(github.event.pull_request.body, '[perf-win]') }} steps: - - uses: actions/checkout@v3 + - name: Trigger opensim-perf workflow + uses: Codex-/return-dispatch@v1.12.0 + id: return_dispatch with: - ref: ${{ github.event.pull_request.base.ref }} + token: ${{ secrets.PERF_DISPATCH_PAT }} + repo: opensim-perf + owner: opensim-org + ref: main + workflow: performance_analysis_win.yml + workflow_inputs: '{"commit_sha": "${{ github.event.pull_request.head.sha }}", "target_artifact_name": "opensim-core-${{ github.event.pull_request.base.ref }}-win2022", "source_artifact_name": "opensim-core-${{ needs.windows2022.outputs.version }}-win2022", "target_artifact_zip": "opensim-core-${{ github.event.pull_request.base.ref }}.zip", "source_artifact_zip": "opensim-core-${{ needs.windows2022.outputs.version }}.zip"}' + + - name: Await opensim-perf workflow + uses: Codex-/await-remote-run@v1.11.0 + with: + token: ${{ secrets.PERF_DISPATCH_PAT }} + repo: opensim-perf + owner: opensim-org + run_id: ${{ steps.return_dispatch.outputs.run_id }} + run_timeout_seconds: 10000 + - name: Download performance analysis results + id: download-artifact + uses: dawidd6/action-download-artifact@v3.0.0 + with: + github_token: ${{ secrets.PERF_DISPATCH_PAT }} + run_id: ${{ steps.return_dispatch.outputs.run_id }} + workflow: performance_analysis_win.yml + name: performance-results + path: results + repo: opensim-org/opensim-perf + + - name: Post results to pull request description + run: | + PR_NUMBER=$(jq --raw-output .pull_request.number "$GITHUB_EVENT_PATH") + EXISTING_COMMENT_CONTENT=$(gh api /repos/opensim-org/opensim-core/pulls/${PR_NUMBER} | jq -r '.body') + CUSTOM_HEADER="### Performance analysis" + + REVIEWABLE_TAG="" + REVIEWABLE_CONTENT="" + if [[ $EXISTING_COMMENT_CONTENT == *"$REVIEWABLE_TAG"* ]]; then + REVIEWABLE_CONTENT=$(echo "$EXISTING_COMMENT_CONTENT" | awk "/$REVIEWABLE_TAG/,0") + fi + + if [[ $EXISTING_COMMENT_CONTENT == *"$CUSTOM_HEADER"* ]]; then + CONTENT_TO_REPLACE=$(echo "$EXISTING_COMMENT_CONTENT" | awk "/$CUSTOM_HEADER/,0") + else + CONTENT_TO_REPLACE=${REVIEWABLE_CONTENT} + fi + + NEW_CONTENT="${CUSTOM_HEADER} + + Platform: Windows, self-hosted runner + + $(cat results/performance_results.md) + + ${REVIEWABLE_CONTENT} + " + NEW_COMMENT_BODY="${EXISTING_COMMENT_CONTENT/"${CONTENT_TO_REPLACE}"/${NEW_CONTENT}}" + + # Update the existing description + gh api \ + --method PATCH \ + -H "Content-Type: application/json" \ + -H "Accept: application/vnd.github.v3+json" \ + -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ + repos/opensim-org/opensim-core/pulls/${PR_NUMBER} \ + -f "body=${NEW_COMMENT_BODY}" + + mac_x86_64: + runs-on: macos-13 + name: Mac x86_64 + + outputs: + version: ${{ steps.configure.outputs.version }} + + steps: + - uses: actions/checkout@v4 + - name: Install Python packages uses: actions/setup-python@v4 with: python-version: '3.10' - + - name: Install Homebrew packages # Save the gfortran version to a file so we can use it in the cache key. run: | @@ -478,9 +521,9 @@ jobs: make doxygen make install cd $GITHUB_WORKSPACE - mv $GITHUB_WORKSPACE/../opensim-core-install opensim-core-${{ github.event.pull_request.base.ref }} - zip --symlinks --recurse-paths --quiet opensim-core-${{ github.event.pull_request.base.ref }}.zip opensim-core-${{ github.event.pull_request.base.ref }} - mv opensim-core-${{ github.event.pull_request.base.ref }} $GITHUB_WORKSPACE/../opensim-core-install + mv $GITHUB_WORKSPACE/../opensim-core-install opensim-core-${{ steps.configure.outputs.version }} + zip --symlinks --recurse-paths --quiet opensim-core-${{ steps.configure.outputs.version }}.zip opensim-core-${{ steps.configure.outputs.version }} + mv opensim-core-${{ steps.configure.outputs.version }} $GITHUB_WORKSPACE/../opensim-core-install - name: Test Python bindings run: | @@ -493,20 +536,18 @@ jobs: with: # The upload-artifact zipping does not preserve symlinks or executable # bits. So we zip ourselves, even though this causes a double-zip. - name: opensim-core-${{ github.event.pull_request.base.ref }}-mac - path: opensim-core-${{ github.event.pull_request.base.ref }}.zip - - mac_source: + name: opensim-core-${{ steps.configure.outputs.version }}-mac-x86_64 + path: opensim-core-${{ steps.configure.outputs.version }}.zip - runs-on: macos-12 - - name: ${{ (contains(github.event.pull_request.title, '[perf-mac]') || contains(github.event.pull_request.body, '[perf-mac]')) && 'Mac [source]' || 'Mac' }} + mac_arm64: + runs-on: macos-14 + name: Mac Arm64 outputs: version: ${{ steps.configure.outputs.version }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install Python packages uses: actions/setup-python@v4 @@ -622,107 +663,150 @@ jobs: with: # The upload-artifact zipping does not preserve symlinks or executable # bits. So we zip ourselves, even though this causes a double-zip. - name: opensim-core-${{ steps.configure.outputs.version }}-mac + name: opensim-core-${{ steps.configure.outputs.version }}-mac-arm64 path: opensim-core-${{ steps.configure.outputs.version }}.zip - performance_analysis_win: - name: Performance Analysis (Windows) - - env: - GH_TOKEN: ${{ github.token }} - - needs: - - windows2022_target - - windows2022_source - - runs-on: macos-12 + mac_arm64_perf: + runs-on: macos-14 + name: Mac Arm64 [main] - if: ${{ contains(github.event.pull_request.title, '[perf-win]') || contains(github.event.pull_request.body, '[perf-win]') }} + if: ${{ contains(github.event.pull_request.title, '[perf-mac]') || contains(github.event.pull_request.body, '[perf-mac]') }} steps: - - name: Trigger opensim-perf workflow - uses: Codex-/return-dispatch@v1.12.0 - id: return_dispatch + - uses: actions/checkout@v4 with: - token: ${{ secrets.PERF_DISPATCH_PAT }} - repo: opensim-perf - owner: opensim-org - ref: main - workflow: performance_analysis_win.yml - workflow_inputs: '{"commit_sha": "${{ github.event.pull_request.head.sha }}", "target_artifact_name": "opensim-core-${{ github.event.pull_request.base.ref }}-win", "source_artifact_name": "opensim-core-${{ needs.windows2022_source.outputs.version }}-win", "target_artifact_zip": "opensim-core-${{ github.event.pull_request.base.ref }}.zip", "source_artifact_zip": "opensim-core-${{ needs.windows2022_source.outputs.version }}.zip"}' + ref: ${{ github.event.pull_request.base.ref }} - - name: Await opensim-perf workflow - uses: Codex-/await-remote-run@v1.11.0 + - name: Install Python packages + uses: actions/setup-python@v4 with: - token: ${{ secrets.PERF_DISPATCH_PAT }} - repo: opensim-perf - owner: opensim-org - run_id: ${{ steps.return_dispatch.outputs.run_id }} - run_timeout_seconds: 10000 + python-version: '3.10' + + - name: Install Homebrew packages + # Save the gfortran version to a file so we can use it in the cache key. + run: | + brew install cmake pkgconfig autoconf libtool automake wget pcre doxygen llvm + brew reinstall gcc + pip3 install numpy==1.25 + gfortran -v + mkdir gfortran_version + gfortran -v &> gfortran_version/gfortran_version.txt - - name: Download performance analysis results - id: download-artifact - uses: dawidd6/action-download-artifact@v3.0.0 + - name: Cache SWIG + id: cache-swig + uses: actions/cache@v3 with: - github_token: ${{ secrets.PERF_DISPATCH_PAT }} - run_id: ${{ steps.return_dispatch.outputs.run_id }} - workflow: performance_analysis_win.yml - name: performance-results - path: results - repo: opensim-org/opensim-perf + path: ~/swig + key: ${{ runner.os }}-swig - - name: Post results to pull request description + - name: Install SWIG + # if: steps.cache-swig.outputs.cache-hit != 'true' run: | - PR_NUMBER=$(jq --raw-output .pull_request.number "$GITHUB_EVENT_PATH") - EXISTING_COMMENT_CONTENT=$(gh api /repos/opensim-org/opensim-core/pulls/${PR_NUMBER} | jq -r '.body') - CUSTOM_HEADER="### Performance analysis" + mkdir ~/swig-source && cd ~/swig-source + wget https://github.com/swig/swig/archive/refs/tags/v4.1.1.tar.gz + tar xzf v4.1.1.tar.gz && cd swig-4.1.1 + sh autogen.sh && ./configure --prefix=$HOME/swig --disable-ccache + make && make -j4 install - REVIEWABLE_TAG="" - REVIEWABLE_CONTENT="" - if [[ $EXISTING_COMMENT_CONTENT == *"$REVIEWABLE_TAG"* ]]; then - REVIEWABLE_CONTENT=$(echo "$EXISTING_COMMENT_CONTENT" | awk "/$REVIEWABLE_TAG/,0") - fi + - name: Cache dependencies + id: cache-dependencies + uses: actions/cache@v3 + with: + path: ~/opensim_dependencies_install + # If Homebrew updates the gcc package, then our cache of IPOPT is invalid. + # Specifically, the pkgcfg_lib_IPOPT_gfortran CMake variable becomes + # undefined. + key: ${{ runner.os }}-dependencies-${{ hashFiles('dependencies/*') }}-${{ hashFiles('gfortran_version/*') }} - if [[ $EXISTING_COMMENT_CONTENT == *"$CUSTOM_HEADER"* ]]; then - CONTENT_TO_REPLACE=$(echo "$EXISTING_COMMENT_CONTENT" | awk "/$CUSTOM_HEADER/,0") - else - CONTENT_TO_REPLACE=${REVIEWABLE_CONTENT} - fi + - name: Build dependencies + if: steps.cache-dependencies.outputs.cache-hit != 'true' + run: | + mkdir $GITHUB_WORKSPACE/../build_deps + cd $GITHUB_WORKSPACE/../build_deps + DEP_CMAKE_ARGS=($GITHUB_WORKSPACE/dependencies -LAH) + DEP_CMAKE_ARGS+=(-DCMAKE_INSTALL_PREFIX=~/opensim_dependencies_install) + DEP_CMAKE_ARGS+=(-DCMAKE_BUILD_TYPE=Release) + DEP_CMAKE_ARGS+=(-DSUPERBUILD_ezc3d=ON) + DEP_CMAKE_ARGS+=(-DOPENSIM_WITH_TROPTER=ON) + DEP_CMAKE_ARGS+=(-DOPENSIM_WITH_CASADI=ON) + DEP_CMAKE_ARGS+=(-DOPENSIM_DISABLE_LOG_FILE=ON) + DEP_CMAKE_ARGS+=(-DCMAKE_OSX_DEPLOYMENT_TARGET=11) + printf '%s\n' "${DEP_CMAKE_ARGS[@]}" + cmake "${DEP_CMAKE_ARGS[@]}" + make --jobs 4 - NEW_CONTENT="${CUSTOM_HEADER} + - name: Configure opensim-core + id: configure + run: | + mkdir $GITHUB_WORKSPACE/../build + cd $GITHUB_WORKSPACE/../build + OSIM_CMAKE_ARGS=($GITHUB_WORKSPACE -LAH) + OSIM_CMAKE_ARGS+=(-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/../opensim-core-install) + OSIM_CMAKE_ARGS+=(-DCMAKE_BUILD_TYPE=Release) + OSIM_CMAKE_ARGS+=(-DOPENSIM_DEPENDENCIES_DIR=~/opensim_dependencies_install) + OSIM_CMAKE_ARGS+=(-DCMAKE_OSX_DEPLOYMENT_TARGET=11) + OSIM_CMAKE_ARGS+=(-DOPENSIM_C3D_PARSER=ezc3d) + OSIM_CMAKE_ARGS+=(-DBUILD_PYTHON_WRAPPING=on -DBUILD_JAVA_WRAPPING=on) + OSIM_CMAKE_ARGS+=(-DSWIG_EXECUTABLE=$HOME/swig/bin/swig) + OSIM_CMAKE_ARGS+=(-DOPENSIM_INSTALL_UNIX_FHS=OFF) + OSIM_CMAKE_ARGS+=(-DOPENSIM_DOXYGEN_USE_MATHJAX=off) + # TODO: Update to simbody.github.io/latest + OSIM_CMAKE_ARGS+=(-DOPENSIM_SIMBODY_DOXYGEN_LOCATION="https://simbody.github.io/simtk.org/api_docs/simbody/latest/") + OSIM_CMAKE_ARGS+=(-DCMAKE_CXX_FLAGS="-Werror, -Wdeprecated-copy") + printf '%s\n' "${OSIM_CMAKE_ARGS[@]}" + cmake "${OSIM_CMAKE_ARGS[@]}" + VERSION=`cmake -L . | grep OPENSIM_QUALIFIED_VERSION | cut -d "=" -f2` + echo $VERSION + echo "VERSION=$VERSION" >> $GITHUB_ENV + echo "version=$VERSION" >> $GITHUB_OUTPUT - Platform: Windows, self-hosted runner + - name: Build opensim-core + run: | + cd $GITHUB_WORKSPACE/../build + make --jobs 4 - $(cat results/performance_results.md) + - name: Test opensim-core + run: | + cd $GITHUB_WORKSPACE/../build + ctest --parallel 4 --output-on-failure - ${REVIEWABLE_CONTENT} - " - NEW_COMMENT_BODY="${EXISTING_COMMENT_CONTENT/"${CONTENT_TO_REPLACE}"/${NEW_CONTENT}}" + - name: Install opensim-core + run: | + cd $GITHUB_WORKSPACE/../build + make doxygen + make install + cd $GITHUB_WORKSPACE + mv $GITHUB_WORKSPACE/../opensim-core-install opensim-core-${{ github.event.pull_request.base.ref }} + zip --symlinks --recurse-paths --quiet opensim-core-${{ github.event.pull_request.base.ref }}.zip opensim-core-${{ github.event.pull_request.base.ref }} + mv opensim-core-${{ github.event.pull_request.base.ref }} $GITHUB_WORKSPACE/../opensim-core-install - # Update the existing description - gh api \ - --method PATCH \ - -H "Content-Type: application/json" \ - -H "Accept: application/vnd.github.v3+json" \ - -H "Authorization: Bearer ${{ secrets.GITHUB_TOKEN }}" \ - repos/opensim-org/opensim-core/pulls/${PR_NUMBER} \ - -f "body=${NEW_COMMENT_BODY}" + - name: Test Python bindings + run: | + cd $GITHUB_WORKSPACE/../opensim-core-install/sdk/Python + # Run the python tests, verbosely. + python3 -m unittest discover --start-directory opensim/tests --verbose - performance_analysis_mac: + - name: Upload opensim-core + uses: actions/upload-artifact@v4 + with: + # The upload-artifact zipping does not preserve symlinks or executable + # bits. So we zip ourselves, even though this causes a double-zip. + name: opensim-core-${{ github.event.pull_request.base.ref }}-mac-arm64 + path: opensim-core-${{ github.event.pull_request.base.ref }}.zip - name: Performance Analysis (Mac) + performance_analysis_mac: + runs-on: ubuntu-latest + name: Performance Analysis [Mac Arm64] env: GH_TOKEN: ${{ github.token }} - needs: - - mac_target - - mac_source - - runs-on: macos-12 - if: ${{ contains(github.event.pull_request.title, '[perf-mac]') || contains(github.event.pull_request.body, '[perf-mac]') }} + needs: + - mac_arm64 + - mac_arm64_perf + steps: - name: Trigger opensim-perf workflow uses: Codex-/return-dispatch@v1.12.0 @@ -733,7 +817,7 @@ jobs: owner: opensim-org ref: main workflow: performance_analysis_mac.yml - workflow_inputs: '{"commit_sha": "${{ github.event.pull_request.head.sha }}", "target_artifact_name": "opensim-core-${{ github.event.pull_request.base.ref }}-mac", "source_artifact_name": "opensim-core-${{ needs.mac_source.outputs.version }}-mac", "target_artifact_zip": "opensim-core-${{ github.event.pull_request.base.ref }}.zip", "source_artifact_zip": "opensim-core-${{ needs.mac_source.outputs.version }}.zip"}' + workflow_inputs: '{"commit_sha": "${{ github.event.pull_request.head.sha }}", "target_artifact_name": "opensim-core-${{ github.event.pull_request.base.ref }}-mac-arm64", "source_artifact_name": "opensim-core-${{ needs.mac_arm64.outputs.version }}-mac-arm64", "target_artifact_zip": "opensim-core-${{ github.event.pull_request.base.ref }}.zip", "source_artifact_zip": "opensim-core-${{ needs.mac_arm64.outputs.version }}.zip"}' - name: Await opensim-perf workflow uses: Codex-/await-remote-run@v1.11.0 @@ -793,12 +877,11 @@ jobs: -f "body=${NEW_COMMENT_BODY}" ubuntu20: - name: Ubuntu2004 - + name: Ubuntu 20.04 runs-on: ubuntu-20.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install Python packages uses: actions/setup-python@v4 @@ -888,31 +971,29 @@ jobs: make --jobs 4 install cd $GITHUB_WORKSPACE mv $GITHUB_WORKSPACE/../opensim-core-install opensim-core-${{ steps.configure.outputs.version }} - zip --symlinks --recurse-paths --quiet opensim-core-${{ steps.configure.outputs.version }}-ub20.zip opensim-core-${{ steps.configure.outputs.version }} + zip --symlinks --recurse-paths --quiet opensim-core-${{ steps.configure.outputs.version }}-ubuntu20.zip opensim-core-${{ steps.configure.outputs.version }} mv opensim-core-${{ steps.configure.outputs.version }} $GITHUB_WORKSPACE/../opensim-core-install # - name: Test Python bindings # run: | - # echo "TODO: Skipping Python tests." - # # cd $GITHUB_WORKSPACE/../opensim-core-install/sdk/Python + # cd $GITHUB_WORKSPACE/../opensim-core-install/sdk/Python # # Run the python tests, verbosely. - # # python3 -m unittest discover --start-directory opensim/tests --verbose + # python3 -m unittest discover --start-directory opensim/tests --verbose - name: Upload opensim-core uses: actions/upload-artifact@v4 with: # The upload-artifact zipping does not preserve symlinks or executable # bits. So we zip ourselves, even though this causes a double-zip. - name: opensim-core-${{ steps.configure.outputs.version }}-ub20-linux - path: opensim-core-${{ steps.configure.outputs.version }}-ub20.zip - - ubuntu20-nomoco: - name: Ubuntu2004-nomoco + name: opensim-core-${{ steps.configure.outputs.version }}-ubuntu20-linux + path: opensim-core-${{ steps.configure.outputs.version }}-ubuntu20.zip - runs-on: ubuntu-20.04 + ubuntu22: + name: Ubuntu 22.04 + runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install Python packages uses: actions/setup-python@v4 @@ -935,6 +1016,7 @@ jobs: tar xzf v4.1.1.tar.gz && cd swig-4.1.1 sh autogen.sh && ./configure --prefix=$HOME/swig --disable-ccache make && make -j4 install + - name: Cache dependencies id: cache-dependencies uses: actions/cache@v3 @@ -943,7 +1025,7 @@ jobs: key: ${{ runner.os }}-dependencies-${{ hashFiles('dependencies/*') }} - name: Build dependencies - if: steps.cache-dependencies.outputs.cache-hit != 'true' + # if: steps.cache-dependencies.outputs.cache-hit != 'true' run: | mkdir $GITHUB_WORKSPACE/../build_deps cd $GITHUB_WORKSPACE/../build_deps @@ -951,8 +1033,8 @@ jobs: DEP_CMAKE_ARGS+=(-DCMAKE_INSTALL_PREFIX=~/opensim_dependencies_install) DEP_CMAKE_ARGS+=(-DCMAKE_BUILD_TYPE=Release) DEP_CMAKE_ARGS+=(-DSUPERBUILD_ezc3d=ON) - DEP_CMAKE_ARGS+=(-DOPENSIM_WITH_TROPTER=OFF) - DEP_CMAKE_ARGS+=(-DOPENSIM_WITH_CASADI=OFF) + DEP_CMAKE_ARGS+=(-DOPENSIM_WITH_TROPTER=ON) + DEP_CMAKE_ARGS+=(-DOPENSIM_WITH_CASADI=ON) printf '%s\n' "${DEP_CMAKE_ARGS[@]}" cmake "${DEP_CMAKE_ARGS[@]}" make --jobs 4 @@ -972,8 +1054,6 @@ jobs: OSIM_CMAKE_ARGS+=(-DSWIG_EXECUTABLE=~/swig/bin/swig) OSIM_CMAKE_ARGS+=(-DOPENSIM_INSTALL_UNIX_FHS=OFF) OSIM_CMAKE_ARGS+=(-DOPENSIM_DOXYGEN_USE_MATHJAX=off) - OSIM_CMAKE_ARGS+=(-DOPENSIM_WITH_TROPTER=off) - OSIM_CMAKE_ARGS+=(-DOPENSIM_WITH_CASADI=off) # TODO: Update to simbody.github.io/latest OSIM_CMAKE_ARGS+=(-DOPENSIM_SIMBODY_DOXYGEN_LOCATION="https://simbody.github.io/simtk.org/api_docs/simbody/latest/") OSIM_CMAKE_ARGS+=(-DCMAKE_CXX_FLAGS="-Werror") @@ -994,7 +1074,7 @@ jobs: cd $GITHUB_WORKSPACE/../build # TODO: Temporary for python to find Simbody libraries. export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$HOME/opensim_dependencies_install/simbody/lib - # ctest --parallel 4 --output-on-failure + ctest --parallel 4 --output-on-failure - name: Install opensim-core run: | @@ -1003,64 +1083,62 @@ jobs: make --jobs 4 install cd $GITHUB_WORKSPACE mv $GITHUB_WORKSPACE/../opensim-core-install opensim-core-${{ steps.configure.outputs.version }} - zip --symlinks --recurse-paths --quiet opensim-core-${{ steps.configure.outputs.version }}-ub20.zip opensim-core-${{ steps.configure.outputs.version }} + zip --symlinks --recurse-paths --quiet opensim-core-${{ steps.configure.outputs.version }}-ubuntu22.zip opensim-core-${{ steps.configure.outputs.version }} mv opensim-core-${{ steps.configure.outputs.version }} $GITHUB_WORKSPACE/../opensim-core-install # - name: Test Python bindings # run: | - # echo "TODO: Skipping Python tests." - # # cd $GITHUB_WORKSPACE/../opensim-core-install/sdk/Python + # cd $GITHUB_WORKSPACE/../opensim-core-install/sdk/Python # # Run the python tests, verbosely. - # # python3 -m unittest discover --start-directory opensim/tests --verbose + # python3 -m unittest discover --start-directory opensim/tests --verbose - name: Upload opensim-core uses: actions/upload-artifact@v4 with: # The upload-artifact zipping does not preserve symlinks or executable # bits. So we zip ourselves, even though this causes a double-zip. - name: opensim-core-${{ steps.configure.outputs.version }}-ub20-nomoco-linux - path: opensim-core-${{ steps.configure.outputs.version }}-ub20.zip - - windows2022-gui: - name: Windows2022-gui - - runs-on: windows-2022 + name: opensim-core-${{ steps.configure.outputs.version }}-ubuntu22-linux + path: opensim-core-${{ steps.configure.outputs.version }}-ubuntu22.zip + + build_gui: + name: Build GUI + runs-on: ubuntu-latest - env: - GH_TOKEN: ${{ github.token }} + env: + GH_TOKEN: ${{ github.token }} - if: ${{ contains(github.event.pull_request.title, '[build-gui]') || contains(github.event.pull_request.body, '[build-gui]') }} + if: ${{ contains(github.event.pull_request.title, '[build-gui]') || contains(github.event.pull_request.body, '[build-gui]') }} - steps: - - name: Dispatch opensim-gui workflow and get the run ID - uses: codex-/return-dispatch@v1 - id: return_dispatch - with: - token: ${{ secrets.ACTION_SECRET }} - ref: refs/heads/main - repo: opensim-gui - owner: opensim-org - workflow: build-on-core-pr.yml - - - name: Use the output run ID - run: echo ${{steps.return_dispatch.outputs.run_id}} - - - name: Await opensim-gui workflow - uses: Codex-/await-remote-run@v1.11.0 - with: - token: ${{ secrets.ACTION_SECRET }} - repo: opensim-gui - owner: opensim-org - run_id: ${{ steps.return_dispatch.outputs.run_id }} - run_timeout_seconds: 10000 + steps: + - name: Dispatch opensim-gui workflow and get the run ID + uses: codex-/return-dispatch@v1 + id: return_dispatch + with: + token: ${{ secrets.ACTION_SECRET }} + ref: refs/heads/main + repo: opensim-gui + owner: opensim-org + workflow: build-on-core-pr.yml + - name: Use the output run ID + run: echo ${{steps.return_dispatch.outputs.run_id}} + + - name: Await opensim-gui workflow + uses: Codex-/await-remote-run@v1.11.0 + with: + token: ${{ secrets.ACTION_SECRET }} + repo: opensim-gui + owner: opensim-org + run_id: ${{ steps.return_dispatch.outputs.run_id }} + run_timeout_seconds: 10000 + style: name: Style - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Check for tabs # Ensure that there are no tabs in source code. diff --git a/.gitignore b/.gitignore index df8f7399cc..5d979dd5ab 100644 --- a/.gitignore +++ b/.gitignore @@ -56,6 +56,7 @@ delete_this_to_stop_optimization*.txt .idea/codeStyles/codeStyleConfig.xml .idea/codeStyles/Project.xml .idea/editor.xml +.idea/copilot/** .cache/clangd/** **/.vscode/ @@ -71,6 +72,8 @@ delete_this_to_stop_optimization*.txt .idea/deployment.xml .idea/other.xml +.idea/encodings.xml +.idea/inspectionProfiles/Project_Default.xml # Clang-Tidy .clang-tidy @@ -80,4 +83,3 @@ cmake-build-* ## File-based project format: *.iws -.vscode/settings.json diff --git a/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m b/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m index f0565655a6..43f9078c40 100644 --- a/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m +++ b/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m @@ -324,19 +324,15 @@ function muscleDrivenJointMomentTracking() % Ignore coordinates that are locked, prescribed, or coupled to other % coordinates via CoordinateCouplerConstraints (true by default). jointMomentTracking.setIgnoreConstrainedCoordinates(true); -coordinateSet = model.getCoordinateSet(); -for i = 0:coordinateSet.getSize()-1 - coordinate = coordinateSet.get(i); - coordName = coordinate.getName(); - % Don't track generalized forces associated with pelvis residuals. - if contains(string(coordName), "pelvis") - jointMomentTracking.setWeightForCoordinate(coordName, 0); - end - % Encourage better tracking of the ankle joint moments. - if contains(string(coordName), "ankle") - jointMomentTracking.setWeightForCoordinate(coordName, 100); - end -end + +% Do not track generalized forces associated with pelvis residuals. +jointMomentTracking.setWeightForGeneralizedForcePattern(".*pelvis.*", 0); + +% Encourage better tracking of the ankle joint moments. +jointMomentTracking.setWeightForGeneralizedForce("ankle_angle_r_moment", 100); +jointMomentTracking.setWeightForGeneralizedForce("ankle_angle_l_moment", 100); + +% Add the joint moment tracking goal to the problem. problem.addGoal(jointMomentTracking); % Update the solver problem and tolerances. diff --git a/Bindings/Java/Matlab/tests/testConnectorsInputsOutputs.m b/Bindings/Java/Matlab/tests/testConnectorsInputsOutputs.m index 8061e53604..07a5b21a6e 100644 --- a/Bindings/Java/Matlab/tests/testConnectorsInputsOutputs.m +++ b/Bindings/Java/Matlab/tests/testConnectorsInputsOutputs.m @@ -13,8 +13,8 @@ source = TableSource(); source.setName('source'); table = TimeSeriesTable(); -labels = StdVectorString(4); -labels.set(0, 'c1'); labels.set(1, 'c2'); labels.set(2, 'c3'); labels.set(3, 'c4'); +labels = StdVectorString(); +labels.add('c1'); labels.add('c2'); labels.add('c3'); labels.add('c4'); table.setColumnLabels(labels); row = RowVector(4, 1.0); table.appendRow(0.0, row); diff --git a/Bindings/Java/Matlab/tests/testOsimC3D.m b/Bindings/Java/Matlab/tests/testOsimC3D.m index 8efd3547dd..7f118cb115 100644 --- a/Bindings/Java/Matlab/tests/testOsimC3D.m +++ b/Bindings/Java/Matlab/tests/testOsimC3D.m @@ -48,8 +48,8 @@ % col numbers. % Test Marker Rotations -randomRows = randsample(markers.getNumRows(),10)-1; -randomCols = randsample(markers.getNumColumns(),10)-1; +randomRows = randi(markers.getNumRows(),10)-1; +randomCols = randi(markers.getNumColumns(),10)-1; for i = 1 : 10 % Get the values in the original and rotated marker tables loc = osimVec3ToArray(markers.getRowAtIndex(randomRows(i)).get(randomCols(i))); @@ -68,8 +68,8 @@ end % Test Force Rotations -randomRows = randsample(forces.getNumRows(),6)-1; -randomCols = randsample(forces.getNumColumns(),6)-1; +randomRows = randi(forces.getNumRows(),6)-1; +randomCols = randi(forces.getNumColumns(),6)-1; for i = 1 : 6 % Get the values in the original and rotated marker tables val = osimVec3ToArray(forces.getRowAtIndex(randomRows(i)).get(randomCols(i))); @@ -93,7 +93,7 @@ % Forces should remain unchanged, point and moments should be in meters % (divided by 1000) -randomRows = randsample(forces.getNumRows(),101)-1; +randomRows = randi(forces.getNumRows(),101)-1; for i = 0 : forces_m.getNumColumns() - 1 col = forces_rotated.getDependentColumnAtIndex(i); @@ -106,17 +106,18 @@ d = col.get(randomRows(u)); d_m = col_m.get(randomRows(u)); - assert(d.get(0)==d_m.get(0),'Force Data has been incorrectly altered'); - assert(d.get(1)==d_m.get(1),'Force Data has been incorrectly altered') - assert(d.get(2)==d_m.get(2),'Force Data has been incorrectly altered') + assert(abs(d.get(0)-d_m.get(0))<.001,'Force Data has been incorrectly altered'); + assert(abs(d.get(1)-d_m.get(1))<.001,'Force Data has been incorrectly altered'); + assert(abs(d.get(2)-d_m.get(2))<.001,'Force Data has been incorrectly altered'); + else % Get the values d = col.get(randomRows(u)); d_m = col_m.get(randomRows(u)); - - assert(d.get(0)/1000==d_m.get(0),'Point or Moment Data has been incorrectly altered'); - assert(d.get(1)/1000==d_m.get(1),'Point or Moment Data has been incorrectly altered'); - assert(d.get(2)/1000==d_m.get(2),'Point or Moment Data has been incorrectly altered'); + assert(abs(d.get(0)/1000-d_m.get(0))<.001,'Point or Moment Data has been incorrectly altered'); + assert(abs(d.get(1)/1000-d_m.get(1))<.001,'Point or Moment Data has been incorrectly altered'); + assert(abs(d.get(2)/1000-d_m.get(2))<.001,'Point or Moment Data has been incorrectly altered'); + end end end diff --git a/Bindings/Java/tests/TestMocoSlidingMass.java b/Bindings/Java/tests/TestMocoSlidingMass.java index ffbf9c4466..8bd543d162 100644 --- a/Bindings/Java/tests/TestMocoSlidingMass.java +++ b/Bindings/Java/tests/TestMocoSlidingMass.java @@ -82,7 +82,7 @@ public static void testMocoSlidingMass() throws Exception { // ===================== if (MocoCasADiSolver.isAvailable()) { MocoCasADiSolver ms = study.initCasADiSolver(); - ms.set_num_mesh_intervals(100); + ms.set_num_mesh_intervals(50); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/Bindings/Java/tests/TestTables.java b/Bindings/Java/tests/TestTables.java index 4111c5ddc1..03d92669b9 100644 --- a/Bindings/Java/tests/TestTables.java +++ b/Bindings/Java/tests/TestTables.java @@ -265,6 +265,36 @@ public static void test_DataTable() { assert tableFlat.getNumRows() == 3; assert tableFlat.getNumColumns() == 12; System.out.println(tableFlat); + table = new DataTable(); + labels = new StdVectorString(); + labels.add("col0.0"); labels.add("col0.1"); labels.add("col0.2"); + labels.add("col0.3"); labels.add("col0.4"); labels.add("col0.5"); + labels.add("col0.6"); labels.add("col0.7"); labels.add("col0.8"); + labels.add("col1.0"); labels.add("col1.1"); labels.add("col1.2"); + labels.add("col1.3"); labels.add("col1.4"); labels.add("col1.5"); + labels.add("col1.6"); labels.add("col1.7"); labels.add("col1.8"); + table.setColumnLabels(labels); + row = new RowVector(18, 1); + table.appendRow(1, row); + row = new RowVector(18, 2); + table.appendRow(2, row); + row = new RowVector(18, 3); + table.appendRow(3, row); + assert table.getColumnLabels().size() == 18; + assert table.getNumRows() == 3; + assert table.getNumColumns() == 18; + DataTableRotation tableRot = table.packRotation(); + assert tableRot.getColumnLabel(0).equals("col0"); + assert tableRot.getNumRows() == 3; + assert tableRot.getNumColumns() == 2; + System.out.println(tableRot); + tableFlat = tableRot.flatten(); + assert tableFlat.getColumnLabels().size() == 18; + assert tableFlat.getColumnLabel( 0).equals("col0_1"); + assert tableFlat.getColumnLabel(15).equals("col1_7"); + assert tableFlat.getNumRows() == 3; + assert tableFlat.getNumColumns() == 18; + System.out.println(tableFlat); } public static void test_DataTableVec3() { diff --git a/Bindings/OpenSimHeaders_common.h b/Bindings/OpenSimHeaders_common.h index 44c5f343c5..333b4d7a05 100644 --- a/Bindings/OpenSimHeaders_common.h +++ b/Bindings/OpenSimHeaders_common.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include diff --git a/Bindings/OpenSimHeaders_simulation.h b/Bindings/OpenSimHeaders_simulation.h index bd3194e64d..6e839ea3e8 100644 --- a/Bindings/OpenSimHeaders_simulation.h +++ b/Bindings/OpenSimHeaders_simulation.h @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -147,6 +148,7 @@ #include #include +#include #include #include diff --git a/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py b/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py index 61eb1eebde..97c26ecc57 100644 --- a/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py +++ b/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py @@ -279,18 +279,15 @@ def muscleDrivenJointMomentTracking(): # Ignore coordinates that are locked, prescribed, or coupled to other # coordinates via CoordinateCouplerConstraints (true by default). jointMomentTracking.setIgnoreConstrainedCoordinates(True) - coordinateSet = model.getCoordinateSet() - for i in range(coordinateSet.getSize()): - coordinate = coordinateSet.get(i) - coordName = coordinate.getName() - # Don't track generalized forces associated with pelvis residuals. - if 'pelvis' in coordName: - jointMomentTracking.setWeightForCoordinate(coordName, 0) - - # Encourage better tracking of the ankle joint moments. - if 'ankle' in coordName: - jointMomentTracking.setWeightForCoordinate(coordName, 100) - + + # Do not track generalized forces associated with pelvis residuals. + jointMomentTracking.setWeightForGeneralizedForcePattern('.*pelvis.*', 0) + + # Encourage better tracking of the ankle joint moments. + jointMomentTracking.setWeightForGeneralizedForce('ankle_angle_r_moment', 100) + jointMomentTracking.setWeightForGeneralizedForce('ankle_angle_l_moment', 100) + + # Add the joint moment tracking goal to the problem. problem.addGoal(jointMomentTracking) # Update the solver tolerances. diff --git a/Bindings/Python/tests/test_DataTable.py b/Bindings/Python/tests/test_DataTable.py index 8bbe9214bc..9accfe208f 100644 --- a/Bindings/Python/tests/test_DataTable.py +++ b/Bindings/Python/tests/test_DataTable.py @@ -130,7 +130,7 @@ def test_DataTable(self): row[1] == 200 and row[2] == 300 and row[3] == 400) - row0 = table.getRowAtIndex(0) + row0 = table.updRowAtIndex(0) row0[0] = 10 row0[1] = 10 row0[2] = 10 @@ -140,7 +140,7 @@ def test_DataTable(self): row0[1] == 10 and row0[2] == 10 and row0[3] == 10) - row2 = table.getRow(0.3) + row2 = table.updRow(0.3) row2[0] = 20 row2[1] = 20 row2[2] = 20 @@ -152,7 +152,7 @@ def test_DataTable(self): row2[3] == 20) print(table) # Edit columns of the table. - col1 = table.getDependentColumnAtIndex(1) + col1 = table.updDependentColumnAtIndex(1) col1[0] = 30 col1[1] = 30 col1[2] = 30 @@ -160,7 +160,7 @@ def test_DataTable(self): assert (col1[0] == 30 and col1[1] == 30 and col1[2] == 30) - col3 = table.getDependentColumn('3') + col3 = table.updDependentColumn('3') col3[0] = 40 col3[1] = 40 col3[2] = 40 @@ -299,6 +299,40 @@ def test_DataTable(self): assert tableFlat.getNumRows() == 3 assert tableFlat.getNumColumns() == 12 print(tableFlat) + table = osim.DataTable() + table.setColumnLabels(('col0_x', 'col0_y', 'col0_z', + 'col1_x', 'col1_y', 'col1_z', + 'col2_x', 'col2_y', 'col2_z', + 'col3_x', 'col3_y', 'col3_z', + 'col4_x', 'col4_y', 'col4_z', + 'col5_x', 'col5_y', 'col5_z')) + row = osim.RowVector([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) + table.appendRow(1, row) + row = osim.RowVector([2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]) + table.appendRow(2, row) + row = osim.RowVector([3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) + table.appendRow(3, row) + assert len(table.getColumnLabels()) == 18 + assert table.getNumRows() == 3 + assert table.getNumColumns() == 18 + table.setColumnLabels(('col0_0', 'col0_1', 'col0_2', + 'col0_3', 'col0_4', 'col0_5', + 'col0_6', 'col0_7', 'col0_8', + 'col1_0', 'col1_1', 'col1_2', + 'col1_3', 'col1_4', 'col1_5', + 'col1_6', 'col1_7', 'col1_8')) + tableRot = table.packRotation() + tableRot.getColumnLabels() == ('col0', 'col1') + tableRot.getNumRows() == 3 + tableRot.getNumColumns() == 2 + print(tableRot) + tableFlat = tableRot.flatten() + assert len(tableFlat.getColumnLabels()) == 18 + assert tableFlat.getColumnLabel( 0) == 'col0_1' + assert tableFlat.getColumnLabel(15) == 'col1_7' + assert tableFlat.getNumRows() == 3 + assert tableFlat.getNumColumns() == 18 + print(tableFlat) def test_TimeSeriesTable(self): print() @@ -475,7 +509,7 @@ def test_DataTableVec3(self): print(tableDouble) # Edit rows of the table. - row0 = table.getRowAtIndex(0) + row0 = table.updRowAtIndex(0) row0[0] = osim.Vec3(10, 10, 10) row0[1] = osim.Vec3(10, 10, 10) row0[2] = osim.Vec3(10, 10, 10) @@ -483,7 +517,7 @@ def test_DataTableVec3(self): assert (str(row0[0]) == str(osim.Vec3(10, 10, 10)) and str(row0[1]) == str(osim.Vec3(10, 10, 10)) and str(row0[2]) == str(osim.Vec3(10, 10, 10))) - row2 = table.getRow(0.3) + row2 = table.updRow(0.3) row2[0] = osim.Vec3(20, 20, 20) row2[1] = osim.Vec3(20, 20, 20) row2[2] = osim.Vec3(20, 20, 20) @@ -493,7 +527,7 @@ def test_DataTableVec3(self): str(row2[2]) == str(osim.Vec3(20, 20, 20))) print(table) # Edit columns of the table. - col1 = table.getDependentColumnAtIndex(1) + col1 = table.updDependentColumnAtIndex(1) col1[0] = osim.Vec3(30, 30, 30) col1[1] = osim.Vec3(30, 30, 30) col1[2] = osim.Vec3(30, 30, 30) @@ -501,7 +535,7 @@ def test_DataTableVec3(self): assert (str(col1[0]) == str(osim.Vec3(30, 30, 30)) and str(col1[1]) == str(osim.Vec3(30, 30, 30)) and str(col1[2]) == str(osim.Vec3(30, 30, 30))) - col2 = table.getDependentColumn('2') + col2 = table.updDependentColumn('2') col2[0] = osim.Vec3(40, 40, 40) col2[1] = osim.Vec3(40, 40, 40) col2[2] = osim.Vec3(40, 40, 40) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index eda1bf7f33..f2c65bf9dd 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -35,6 +35,27 @@ def createSlidingMassModel(): return model +def createDoubleSlidingMassModel(): + model = createSlidingMassModel() + body = osim.Body("body2", 10.0, osim.Vec3(0), osim.Inertia(0)) + model.addComponent(body) + + joint = osim.SliderJoint("slider2", model.getGround(), body) + coord = joint.updCoordinate(osim.SliderJoint.Coord_TranslationX) + coord.setName("position"); + model.addComponent(joint); + + actu = osim.CoordinateActuator() + actu.setCoordinate(coord) + actu.setName("actuator2") + actu.setOptimalForce(1) + model.addComponent(actu) + + model.finalizeConnections() + model.initSystem() + return model + + class TestSwigAddtlInterface(unittest.TestCase): def test_bounds(self): model = osim.Model() @@ -391,3 +412,33 @@ def test_changing_costs(self): # Change the weights of the costs. effort.setWeight(0.1) assert(study.solve().getFinalTime() < 0.8 * finalTime0) + + def test_expression_based_parameter_goal(self): + study = osim.MocoStudy() + mp = study.updProblem() + mp.setModel(createDoubleSlidingMassModel()) + mp.setTimeBounds(0, 1) + mp.setStateInfo("/slider/position/value", [-5, 5], 0, [0.2, 0.3]) + mp.setStateInfo("/slider/position/speed", [-20, 20]) + mp.setStateInfo("/slider2/position/value", [-5, 5], 1, [1.2, 1.3]) + mp.setStateInfo("/slider2/position/speed", [-20, 20]) + + parameter = osim.MocoParameter("sphere_mass", "body", "mass", + osim.MocoBounds(0, 10)) + mp.addParameter(parameter) + parameter2 = osim.MocoParameter("sphere2_mass", "body2", "mass", + osim.MocoBounds(0, 10)) + mp.addParameter(parameter2) + total_weight = 7 + mass_goal = osim.MocoExpressionBasedParameterGoal() + mp.addGoal(mass_goal) + mass_goal.setExpression(f"(p+q-{total_weight})^2") + mass_goal.addParameter(parameter, "p") + mass_goal.addParameter(parameter2, "q") + + ms = study.initTropterSolver() + ms.set_num_mesh_intervals(25) + sol = study.solve() + + self.assertAlmostEqual(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass"), + total_weight) diff --git a/Bindings/common.i b/Bindings/common.i index 0b84575d90..73c9d8b7d6 100644 --- a/Bindings/common.i +++ b/Bindings/common.i @@ -60,6 +60,7 @@ %include %include %include +%include %include %include @@ -252,6 +253,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_) packSpatialVec(std::vector suffixes) { return $self->pack(); } + DataTable_> + packRotation() { + return $self->pack>(); + } + DataTable_> + packRotation(std::vector suffixes) { + return $self->pack>(); + } } %ignore OpenSim::TimeSeriesTable_::TimeSeriesTable_(TimeSeriesTable_ &&); @@ -293,6 +302,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_) packSpatialVec(std::vector suffixes) { return $self->pack(); } + TimeSeriesTable_> + packRotation() { + return $self->pack>(); + } + TimeSeriesTable_> + packRotation(std::vector suffixes) { + return $self->pack>(); + } } %extend OpenSim::TimeSeriesTable_ { TimeSeriesTable_ flatten() { @@ -334,6 +351,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_) return $self->flatten(suffixes); } } +%extend OpenSim::TimeSeriesTable_> { + TimeSeriesTable_ flatten() { + return $self->flatten(); + } + TimeSeriesTable_ flatten(std::vector suffixes) { + return $self->flatten(suffixes); + } +} %include %include @@ -370,7 +395,7 @@ DATATABLE_CLONE(double, SimTK::Rotation_) %shared_ptr(OpenSim::IMUDataReader) %shared_ptr(OpenSim::XsensDataReader) %shared_ptr(OpenSim::APDMDataReader) -%shared_ptr(OpenSim::STOFileAdapter_) +%shared_ptr(OpenSim::STOFileAdapter_) %shared_ptr(OpenSim::STOFileAdapter_) %shared_ptr(OpenSim::STOFileAdapter_) %shared_ptr(OpenSim::STOFileAdapter_) diff --git a/Bindings/moco.i b/Bindings/moco.i index 0d66f84c1e..b53d2fa5b3 100644 --- a/Bindings/moco.i +++ b/Bindings/moco.i @@ -24,6 +24,7 @@ namespace OpenSim { %include %include %include +%include %include %include %include @@ -137,6 +138,10 @@ namespace OpenSim { %include %include %include +%template(analyzeVec3) OpenSim::MocoStudy::analyze; +%template(analyzeSpatialVec) OpenSim::MocoStudy::analyze; +%template(analyzeRotation) OpenSim::MocoStudy::analyze>; + %include %include @@ -147,5 +152,6 @@ namespace OpenSim { %template(analyzeMocoTrajectory) OpenSim::analyzeMocoTrajectory; %template(analyzeMocoTrajectoryVec3) OpenSim::analyzeMocoTrajectory; %template(analyzeMocoTrajectorySpatialVec) OpenSim::analyzeMocoTrajectory; +%template(analyzeMocoTrajectoryRotation) OpenSim::analyzeMocoTrajectory>; %include diff --git a/Bindings/simulation.i b/Bindings/simulation.i index bc15cd9c67..b2b0f01fdf 100644 --- a/Bindings/simulation.i +++ b/Bindings/simulation.i @@ -70,6 +70,8 @@ OpenSim::ModelComponentSet; %include %include +%include + %template(SetForces) OpenSim::Set; %template(ModelComponentSetForces) OpenSim::ModelComponentSet; %include @@ -79,6 +81,7 @@ OpenSim::ModelComponentSet; %include %template(TwoFrameLinkerForce) OpenSim::TwoFrameLinker; +%template(TwoFrameLinkerForceProducer) OpenSim::TwoFrameLinker; %template(TwoFrameLinkerConstraint) OpenSim::TwoFrameLinker; %include @@ -250,6 +253,7 @@ OpenSim::ModelComponentSet; %template(StdVectorIMUs) std::vector< OpenSim::IMU* >; +%include %include // This enables iterating using the getBetween() method. %template(IteratorRangeStatesTrajectoryIterator) @@ -260,6 +264,7 @@ OpenSim::ModelComponentSet; %template(analyze) OpenSim::analyze; %template(analyzeVec3) OpenSim::analyze; %template(analyzeSpatialVec) OpenSim::analyze; +%template(analyzeRotation) OpenSim::analyze>; %include diff --git a/CHANGELOG.md b/CHANGELOG.md index 9ce9cffc57..93933e42e0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,10 +12,36 @@ v4.6 the case where provided string is just the name of the value, rather than a path to it (#3782) - Fixed bugs in `MocoStepTimeAsymmetryGoal::printDescriptionImpl()` where there were missing or incorrect values printed. (#3842) - Added `ModOpPrescribeCoordinateValues` which can prescribe motion of joints in a model given a table of data. (#3862) -- Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to - allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from +- Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to + allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from occurring in the output. (#3867) - Added `Output`s to `ExpressionBasedCoordinateForce`, `ExpressionBasedPointToPointForce`, and `ExpressionBasedBushingForce` for accessing force values. (#3872) +- `PointForceDirection` no longer has a virtual destructor, is `final`, and its `scale` functionality + has been marked as `[[deprecated]]` (#3890) +- Added `ExpressionBasedFunction` for creating `Function`s based on user-defined mathematical expressions. (#3892) +- Added `ForceProducer`, `ForceConsumer`, and `ForceApplier`, which are fundamental APIs for force-producing + components. The `ForceProducer` API was also rolled out to a variety of existing `Force` components, which + means that API users can now now ask many `Force` components what forces they produce (see #3891 for a + comprehensive overview). +- Made improvements to `MocoUtilities::createExternalLoadsTableForGait()`: center of pressure values are now set to zero, rather + than NaN, when vertical force is zero, and the vertical torque is returned in the torque columns (rather than the sum of the + sphere torques) to be consistent with the center of pressure GRF representation. +- Fixed an issue where a copy of an `OpenSim::Model` containing a `OpenSim::ExternalLoads` could not be + finalized (#3926) +- Updated all code examples to use c++14 (#3929) +- Added class `OpenSim::StateDocument` as a systematic means of serializing and deserializing a complete trajectory + (i.e., time history) of all states in the `SimTK::State` to and from a single `.ostates` file. Prior to `StatesDocument`, + only the trajectories of continuous states (e.g., joint angles, joint speeds, muscle forces, and the like) could be systematically + written to file, either in the form of a `Storage` file or as a `TimeSeriesTable` file, leaving discrete states (e.g., muscle + excitations and contact model anchor points) and modeling options (e.g., joint clamps) unserialized. `StatesDocument`, on the + other hand, serializes all continuous states, discrete states, and modeling options registered with `OpenSim::Component`. + Whereas neither `Storage` files nor `TimeSeriesTable` files are currently able to handle mixed variable types, `StatesDocument` + is able to accommodate variables of type `bool`, `int`, `double`, `Vec2`, `Vec3`, `Vec4`, `Vec5`, and `Vec6` all in the same + `.ostates` file. `.ostate` files are written in `XML` format and internally carry the name of the OpenSim model to which the + states belong, a date/time stamp of when the file was written, and a user-specified note. `.ostate` files also support a + configurable output precision. At the highest ouput precsion (~20 significant figures), serialization/deserialization is + a lossless process. (#3902) +- Improved `OpenSim::IO::stod` string-to-decimal parsing function by making it not-locale-dependant (#3943, #3924; thanks @alexbeattie42) v4.5.1 ====== @@ -80,6 +106,8 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830) - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834) - Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874). +- Added Python and Java (Matlab) scripting support for `TimeSeriesTable_`. (#3940) +- Added the templatized `MocoStudy::analyze()` and equivalent scripting counterparts: `analyzeVec3`, `analyzeSpatialVec`, `analyzeRotation`. (#3940) v4.5 ==== diff --git a/CHANGELOG_MOCO.md b/CHANGELOG_MOCO.md index 1cf2164ed9..2f19bb1bec 100644 --- a/CHANGELOG_MOCO.md +++ b/CHANGELOG_MOCO.md @@ -3,6 +3,33 @@ Moco Change Log 1.4.0 ----- +- 2024-08-30: Added `MocoInverse::initializeKinematics()` to allow users to retrieve kinematics after + converting the `MocoInverse` to a `MocoStudy`. + +- 2024-08-26: Changed all `printDescription()` and `printDescriptionImpl()` methods to log at level info + instead of cout. + +- 2024-08-21: Added support for the 'projection' method for enforcing kinematic + constraints from Bordalba et al. (2023) to `MocoCasADiSolver`. + This method allows enforcing kinematic constraints with any + transcription scheme and can be enabled using by setting the + property `kinematic_constraint_method` to `'Bordalba2023'`. The + existing default method from Posa et al. (2016) can also be + specified using `'Posa2016'`. + +- 2024-08-21: The `MocoSolver` properties `interpolate_control_midpoints` and + `enforce_path_constraint_midpoints` have been renamed to + `interpolate_control_mesh_interior_points` and + `enforce_path_constraint_mesh_interior_points`, respectively. This + change was made to clarify that these properties apply to the new + Legendre-Gauss and Legendre-Gauss-Radau transcription schemes which + have collocation points in the mesh interval interior that do not + necessarily coincide with the mesh interval midpoints (unlike + Hermite-Simpson transcription). + +- 2024-08-15: Added `MocoExpressionBasedParameterGoal` to enable minimizing any arithmetic expression + of parameter values. + - 2024-07-26: Added `MocoStateBoundConstraint` and `MocoOutputBoundConstraint` to enable bounding state variables or output values by one or two `Function`s, similar to `MocoControlBoundConstraint`. @@ -30,9 +57,9 @@ Moco Change Log (e.g., getInputControlsTrajectory(), generateControlsFromModelControllers()). `MocoGoal`s and `MocoPathConstraint`s have been updated to support Input controls. See the Moco User Guide for details. - + - 2024-04-01: Added `MocoGeneralizedForceTrackingGoal` to enable joint moment tracking - in `MocoProblem`s, and added the utility `calcGeneralizeForces()` to + in `MocoProblem`s, and added the utility `calcGeneralizedForces()` to `MocoStudy` for computing joint moments from a `MocoTrajectory`. Added a sub-example to exampleMocoTrack (C++, Python, and Matlab) to feature this new functionality. diff --git a/OpenSim/Actuators/BodyActuator.cpp b/OpenSim/Actuators/BodyActuator.cpp index 54c820979e..17f87cee26 100644 --- a/OpenSim/Actuators/BodyActuator.cpp +++ b/OpenSim/Actuators/BodyActuator.cpp @@ -21,13 +21,11 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= -#include - #include "BodyActuator.h" +#include +#include + using namespace OpenSim; using namespace std; using SimTK::Vec3; @@ -104,21 +102,41 @@ const Body& BodyActuator::getBody() const return getConnectee("body"); } -//============================================================================== -// APPLICATION -//============================================================================== //_____________________________________________________________________________ /** -* Apply the actuator force/torque to Body. +* Compute power consumed by moving the body via applied spatial force. +* Reads the body spatial velocity vector and spatial force vector applied via +* BodyActuator and computes the power as p = F (dotProdcut) V. */ -void BodyActuator::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +double BodyActuator::getPower(const SimTK::State& s) const +{ + const Body& body = getBody(); + + const SimTK::MobilizedBody& body_mb = body.getMobilizedBody(); + SimTK::SpatialVec bodySpatialVelocities = body_mb.getBodyVelocity(s); + + SimTK::Vector bodyVelocityVec(6); + bodyVelocityVec[0] = bodySpatialVelocities[0][0]; + bodyVelocityVec[1] = bodySpatialVelocities[0][1]; + bodyVelocityVec[2] = bodySpatialVelocities[0][2]; + bodyVelocityVec[3] = bodySpatialVelocities[1][0]; + bodyVelocityVec[4] = bodySpatialVelocities[1][1]; + bodyVelocityVec[5] = bodySpatialVelocities[1][2]; + + const SimTK::Vector bodyForceVals = getControls(s); + + double power = ~bodyForceVals * bodyVelocityVec; + + return power; +} + +void BodyActuator::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const { if (!_model) return; const bool spatialForceIsGlobal = getSpatialForceIsGlobal(); - + const Body& body = getBody(); // const SimTK::MobilizedBody& body_mb = body.getMobilizedBody(); @@ -144,39 +162,8 @@ void BodyActuator::computeForce(const SimTK::State& s, // case) transform it to body frame if (get_point_is_global()) pointOfApplication = getModel().getGround(). - findStationLocationInAnotherFrame(s, pointOfApplication, body); - - applyTorque(s, body, torqueVec, bodyForces); - applyForceToPoint(s, body, pointOfApplication, forceVec, bodyForces); + findStationLocationInAnotherFrame(s, pointOfApplication, body); + forceConsumer.consumeTorque(s, body, torqueVec); + forceConsumer.consumePointForce(s, body, pointOfApplication, forceVec); } - -//_____________________________________________________________________________ -/** -* Compute power consumed by moving the body via applied spatial force. -* Reads the body spatial velocity vector and spatial force vector applied via -* BodyActuator and computes the power as p = F (dotProdcut) V. -*/ -double BodyActuator::getPower(const SimTK::State& s) const -{ - const Body& body = getBody(); - - const SimTK::MobilizedBody& body_mb = body.getMobilizedBody(); - SimTK::SpatialVec bodySpatialVelocities = body_mb.getBodyVelocity(s); - - SimTK::Vector bodyVelocityVec(6); - bodyVelocityVec[0] = bodySpatialVelocities[0][0]; - bodyVelocityVec[1] = bodySpatialVelocities[0][1]; - bodyVelocityVec[2] = bodySpatialVelocities[0][2]; - bodyVelocityVec[3] = bodySpatialVelocities[1][0]; - bodyVelocityVec[4] = bodySpatialVelocities[1][1]; - bodyVelocityVec[5] = bodySpatialVelocities[1][2]; - - const SimTK::Vector bodyForceVals = getControls(s); - - double power = ~bodyForceVals * bodyVelocityVec; - - return power; -} - - diff --git a/OpenSim/Actuators/BodyActuator.h b/OpenSim/Actuators/BodyActuator.h index dd04ac265e..a69b377be3 100644 --- a/OpenSim/Actuators/BodyActuator.h +++ b/OpenSim/Actuators/BodyActuator.h @@ -140,12 +140,10 @@ class OSIMACTUATORS_API BodyActuator : public Actuator { Begin with its properties. */ void constructProperties(); - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + /** + * Implements the `ForceProducer` interface. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; //-------------------------------------------------------------------------- // Implement Actuator interface. diff --git a/OpenSim/Actuators/CoordinateActuator.cpp b/OpenSim/Actuators/CoordinateActuator.cpp index 17c473dfde..debe365ee6 100644 --- a/OpenSim/Actuators/CoordinateActuator.cpp +++ b/OpenSim/Actuators/CoordinateActuator.cpp @@ -22,16 +22,14 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================== -// INCLUDES -//============================================================================== +#include "CoordinateActuator.h" + #include #include #include +#include #include -#include "CoordinateActuator.h" - using namespace OpenSim; using namespace std; @@ -181,26 +179,25 @@ CreateForceSetOfCoordinateActuatorsForModel(const SimTK::State& s, Model& aModel //============================================================================== //_____________________________________________________________________________ /** - * Apply the actuator force to BodyA and BodyB. + * Produces the actuator force for BodyA and BodyB. */ -void CoordinateActuator::computeForce( const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const +void CoordinateActuator::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const { - if(!_model) return; - - double force; - if (isActuationOverridden(s)) { - force = computeOverrideActuation(s); - } else { - force = computeActuation(s); + if (!_model) { + return; } - setActuation(s, force); - if(isCoordinateValid()){ - applyGeneralizedForce(s, *_coord, getActuation(s), mobilityForces); + const double force = isActuationOverridden(s) ? + computeOverrideActuation(s) : + computeActuation(s); + + setActuation(s, force); + + if (isCoordinateValid()) { + forceConsumer.consumeGeneralizedForce(s, *_coord, getActuation(s)); } else { - log_warn("CoordinateActuator::computeForce: Invalid coordinate"); + log_warn("CoordinateActuator::implProduceForces: Invalid coordinate"); } } diff --git a/OpenSim/Actuators/CoordinateActuator.h b/OpenSim/Actuators/CoordinateActuator.h index e0d9b99e5d..59c6ec6a62 100644 --- a/OpenSim/Actuators/CoordinateActuator.h +++ b/OpenSim/Actuators/CoordinateActuator.h @@ -89,12 +89,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CoordinateActuator, ScalarActuator); // PRIVATE //============================================================================== private: - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + /** + * Implements the `ForceProducer` interface. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; //-------------------------------------------------------------------------- diff --git a/OpenSim/Actuators/McKibbenActuator.cpp b/OpenSim/Actuators/McKibbenActuator.cpp index 06719f4c62..3f1a192545 100644 --- a/OpenSim/Actuators/McKibbenActuator.cpp +++ b/OpenSim/Actuators/McKibbenActuator.cpp @@ -91,24 +91,6 @@ double McKibbenActuator::computeActuation( const SimTK::State& s ) const return force; } -//============================================================================== -// APPLICATION -//============================================================================== -//_____________________________________________________________________________ -/** - * Apply the actuator force to the path - * - * @param s current SimTK::State - */ -void McKibbenActuator::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const -{ - - double actuation = computeActuation(s); - - getPath().addInEquivalentForces(s, actuation, bodyForces, generalizedForces); -} //_____________________________________________________________________________ /** * Sets the actual Body references _bodyA and _bodyB diff --git a/OpenSim/Actuators/McKibbenActuator.h b/OpenSim/Actuators/McKibbenActuator.h index a4c0ac164c..caa5a15afd 100644 --- a/OpenSim/Actuators/McKibbenActuator.h +++ b/OpenSim/Actuators/McKibbenActuator.h @@ -96,14 +96,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(McKibbenActuator, PathActuator); private: void constructProperties(); - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; - - //-------------------------------------------------------------------------- // Implement ModelComponent interface //-------------------------------------------------------------------------- diff --git a/OpenSim/Actuators/PointActuator.cpp b/OpenSim/Actuators/PointActuator.cpp index 87e6270480..527db2b6b5 100644 --- a/OpenSim/Actuators/PointActuator.cpp +++ b/OpenSim/Actuators/PointActuator.cpp @@ -25,16 +25,13 @@ * Author: Ajay Seth */ +#include "PointActuator.h" -//============================================================================= -// INCLUDES -//============================================================================= #include +#include #include #include -#include "PointActuator.h" - using namespace OpenSim; using namespace std; using SimTK::Vec3; @@ -180,38 +177,32 @@ double PointActuator::computeActuation(const SimTK::State& s) const return getControl(s) * getOptimalForce(); } -//============================================================================= -// APPLICATION -//============================================================================= -//_____________________________________________________________________________ -/** - * Apply the actuator force to BodyA and BodyB. - */ -void PointActuator::computeForce( +void PointActuator::implProduceForces( const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const + ForceConsumer& forceConsumer) const { if (!_model || !_body) { return; } - double force = isActuationOverridden(s) ? computeOverrideActuation(s) - : computeActuation(s); + const double force = isActuationOverridden(s) ? computeOverrideActuation(s) + : computeActuation(s); setActuation(s, force); Vec3 forceVec = force * SimTK::UnitVec3(get_direction()); - Vec3 lpoint = get_point(); if (!get_force_is_global()) { forceVec = _body->expressVectorInGround(s, forceVec); } + + Vec3 lpoint = get_point(); if (get_point_is_global()) { lpoint = getModel().getGround().findStationLocationInAnotherFrame( s, lpoint, *_body); } - applyForceToPoint(s, *_body, lpoint, forceVec, bodyForces); + + forceConsumer.consumePointForce(s, *_body, lpoint, forceVec); } //_____________________________________________________________________________ /** diff --git a/OpenSim/Actuators/PointActuator.h b/OpenSim/Actuators/PointActuator.h index 6bfa46a40d..4faa4492be 100644 --- a/OpenSim/Actuators/PointActuator.h +++ b/OpenSim/Actuators/PointActuator.h @@ -110,12 +110,11 @@ class OSIMACTUATORS_API PointActuator : public ScalarActuator { /** Computes speed along force vector. */ double calcSpeed(const SimTK::State& s) const; - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + /** + * Implements the `ForceProducer` interface by applying the actuator force to + * BodyA and BodyB. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; //-------------------------------------------------------------------------- // Implement Actuator interface (also see getOptimalForce() above) diff --git a/OpenSim/Actuators/PointToPointActuator.cpp b/OpenSim/Actuators/PointToPointActuator.cpp index b951e542c3..669e3e87c1 100644 --- a/OpenSim/Actuators/PointToPointActuator.cpp +++ b/OpenSim/Actuators/PointToPointActuator.cpp @@ -25,14 +25,12 @@ * Author: Matt DeMers */ -//============================================================================== -// INCLUDES -//============================================================================== +#include "PointToPointActuator.h" + +#include #include #include -#include "PointToPointActuator.h" - using namespace OpenSim; using std::string; using SimTK::Vec3; using SimTK::Vector_; using SimTK::Vector; @@ -226,10 +224,9 @@ double PointToPointActuator::calcSpeed(const SimTK::State& s) const return speed; } -void PointToPointActuator::computeForce( +void PointToPointActuator::implProduceForces( const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const + ForceConsumer& forceConsumer) const { if (!_model || !_bodyA || !_bodyB) { return; @@ -259,9 +256,9 @@ void PointToPointActuator::computeForce( const SimTK::Vec3 force = forceMagnitude * getDirectionBAInGround(s); - // Apply equal and opposite forces to the bodies. - applyForceToPoint(s, *_bodyA, pointA_inBodyA, force, bodyForces); - applyForceToPoint(s, *_bodyB, pointB_inBodyB, -force, bodyForces); + // Produce equal and opposite forces on the body frames. + forceConsumer.consumePointForce(s, *_bodyA, pointA_inBodyA, force); + forceConsumer.consumePointForce(s, *_bodyB, pointB_inBodyB, -force); } //_____________________________________________________________________________ /** diff --git a/OpenSim/Actuators/PointToPointActuator.h b/OpenSim/Actuators/PointToPointActuator.h index 6e3667cab7..9ce28f78c5 100644 --- a/OpenSim/Actuators/PointToPointActuator.h +++ b/OpenSim/Actuators/PointToPointActuator.h @@ -128,12 +128,10 @@ class OSIMACTUATORS_API PointToPointActuator : public ScalarActuator { Body* getBodyA() const {return _bodyA.get();} Body* getBodyB() const {return _bodyB.get();} - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + /** + * Implements the `ForceProducer` interface. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; //-------------------------------------------------------------------------- // Implement Actuator interface (also see getOptimalForce() above) diff --git a/OpenSim/Actuators/SpringGeneralizedForce.cpp b/OpenSim/Actuators/SpringGeneralizedForce.cpp index dfd3faa8e8..de78fcd75b 100644 --- a/OpenSim/Actuators/SpringGeneralizedForce.cpp +++ b/OpenSim/Actuators/SpringGeneralizedForce.cpp @@ -25,6 +25,7 @@ //============================================================================== // INCLUDES //============================================================================== +#include #include #include #include "SpringGeneralizedForce.h" @@ -173,19 +174,18 @@ getStiffness() const //============================================================================== //_____________________________________________________________________________ /** - * Compute all quantities necessary for applying the spring force to the - * model. + * Compute all quantities necessary for producing the spring force. + * * Force applied = -stiffness * (_coordinateValue - restLength) * - viscosity * _coordinateSpeed */ -void SpringGeneralizedForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void SpringGeneralizedForce::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { if( !_model || !_coord ) return; - // FORCE - applyGeneralizedForce(s, *_coord, computeForceMagnitude(s), generalizedForces); + forceConsumer.consumeGeneralizedForce(s, *_coord, computeForceMagnitude(s)); } //_____________________________________________________________________________ /** diff --git a/OpenSim/Actuators/SpringGeneralizedForce.h b/OpenSim/Actuators/SpringGeneralizedForce.h index 8135d61486..00aa4129cf 100644 --- a/OpenSim/Actuators/SpringGeneralizedForce.h +++ b/OpenSim/Actuators/SpringGeneralizedForce.h @@ -27,7 +27,8 @@ // INCLUDES //============================================================================== #include "osimActuatorsDLL.h" -#include + +#include //============================================================================== //============================================================================== @@ -42,8 +43,8 @@ class Coordinate; * @author Frank C. Anderson, Ajay Seth * @version 2.0 */ -class OSIMACTUATORS_API SpringGeneralizedForce : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(SpringGeneralizedForce, Force); +class OSIMACTUATORS_API SpringGeneralizedForce : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(SpringGeneralizedForce, ForceProducer); public: //============================================================================== // PROPERTIES @@ -96,10 +97,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(SpringGeneralizedForce, Force); //-------------------------------------------------------------------------- // COMPUTATIONS protected: - // Force interface. - void computeForce( const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; // ModelComponent interface. void extendAddToSystem(SimTK::MultibodySystem& system) const override; @@ -108,6 +105,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(SpringGeneralizedForce, Force); void extendConnectToModel(Model& model) override; private: + /** + * Implements the `ForceProducer` interface. + */ + void implProduceForces( + const SimTK::State& state, + ForceConsumer& forceConsumer + ) const override; + void setNull(); void constructProperties(); double computeForceMagnitude(const SimTK::State& s) const; diff --git a/OpenSim/Actuators/Test/testDeGrooteFregly2016Muscle.cpp b/OpenSim/Actuators/Test/testDeGrooteFregly2016Muscle.cpp index 07d6951a1c..bd0c3533f0 100644 --- a/OpenSim/Actuators/Test/testDeGrooteFregly2016Muscle.cpp +++ b/OpenSim/Actuators/Test/testDeGrooteFregly2016Muscle.cpp @@ -597,13 +597,16 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { CHECK(muscle.getFiberVelocityAlongTendon(state) == -1 * Vmax); CHECK(muscle.getPennationAngularVelocity(state) == 0); CHECK(muscle.getTendonVelocity(state) == 0); - CHECK(muscle.getForceVelocityMultiplier(state) == 0); + CHECK_THAT(muscle.getForceVelocityMultiplier(state), + Catch::Matchers::WithinAbs(0.0, 1e-10)); model.realizeDynamics(state); const auto Fmax = muscle.getMaxIsometricForce(); const auto fpass = muscle.calcPassiveForceMultiplier(1.0); - CHECK(muscle.getActiveFiberForce(state) == 0); - CHECK(muscle.getActiveFiberForceAlongTendon(state) == 0); + CHECK_THAT(muscle.getActiveFiberForce(state), + Catch::Matchers::WithinAbs(0.0, 1e-10)); + CHECK_THAT(muscle.getActiveFiberForceAlongTendon(state), + Catch::Matchers::WithinAbs(0.0, 1e-10)); CHECK(muscle.getPassiveFiberForce(state) == Approx(Fmax * fpass)); CHECK(muscle.getPassiveFiberForceAlongTendon(state) == Approx(Fmax * fpass)); @@ -630,16 +633,17 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { SimTK::Real fiberStiffnessAlongTendon = diffFiberStiffnessAlongTendon.calcDerivative( muscle.get_optimal_fiber_length()); - CHECK(muscle.getFiberStiffnessAlongTendon(state) == - Approx(fiberStiffnessAlongTendon)); + CHECK_THAT(muscle.getFiberStiffnessAlongTendon(state), + Catch::Matchers::WithinRel(fiberStiffnessAlongTendon, 1e-6)); SimTK::Real tendonStiffness = SimTK::Infinity; CHECK(muscle.getTendonStiffness(state) == SimTK::Infinity); - CHECK(muscle.getMuscleStiffness(state) == - Approx(muscle.calcMuscleStiffness( - tendonStiffness, fiberStiffnessAlongTendon))); - - CHECK(muscle.getFiberActivePower(state) == Approx(0.0)); + SimTK::Real muscleStiffness = muscle.calcMuscleStiffness( + tendonStiffness, fiberStiffnessAlongTendon); + CHECK_THAT(muscle.getMuscleStiffness(state), + Catch::Matchers::WithinRel(muscleStiffness, 1e-6)); + CHECK_THAT(muscle.getFiberActivePower(state), + Catch::Matchers::WithinAbs(0.0, 1e-10)); CHECK(muscle.getFiberPassivePower(state) == Approx(Vmax * Fmax * fpass)); CHECK(muscle.getTendonPower(state) == Approx(0.0)); diff --git a/OpenSim/Actuators/TorqueActuator.cpp b/OpenSim/Actuators/TorqueActuator.cpp index b7ffef3c16..3223d86a03 100644 --- a/OpenSim/Actuators/TorqueActuator.cpp +++ b/OpenSim/Actuators/TorqueActuator.cpp @@ -29,6 +29,8 @@ // INCLUDES //============================================================================== #include "TorqueActuator.h" + +#include #include using namespace OpenSim; @@ -144,12 +146,11 @@ double TorqueActuator::computeActuation(const State& s) const //============================================================================== //_____________________________________________________________________________ /** - * Apply the actuator force to BodyA and BodyB. + * Produce the actuator forces for BodyA and BodyB. */ -void TorqueActuator::computeForce( - const State& s, - Vector_& bodyForces, - Vector& generalizedForces) const +void TorqueActuator::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { if (!_model || !_bodyA) { return; @@ -168,11 +169,11 @@ void TorqueActuator::computeForce( torque = _bodyA->expressVectorInGround(s, torque); } - applyTorque(s, *_bodyA, torque, bodyForces); + forceConsumer.consumeTorque(s, *_bodyA, torque); // if bodyB is not specified, use the ground body by default if (_bodyB) { - applyTorque(s, *_bodyB, -torque, bodyForces); + forceConsumer.consumeTorque(s, *_bodyB, -torque); } } diff --git a/OpenSim/Actuators/TorqueActuator.h b/OpenSim/Actuators/TorqueActuator.h index 5bfcbd69ec..db067dc52f 100644 --- a/OpenSim/Actuators/TorqueActuator.h +++ b/OpenSim/Actuators/TorqueActuator.h @@ -137,12 +137,10 @@ class OSIMACTUATORS_API TorqueActuator : public ScalarActuator { private: void constructProperties(); - //-------------------------------------------------------------------------- - // Implement Force interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + /** + * Implements the `ForceProducer` interface. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; //-------------------------------------------------------------------------- // Implement Actuator interface (also see getOptimalForce() above) diff --git a/OpenSim/Analyses/Test/testOutputReporter.cpp b/OpenSim/Analyses/Test/testOutputReporter.cpp index 13eb7791e5..c1451df6a4 100644 --- a/OpenSim/Analyses/Test/testOutputReporter.cpp +++ b/OpenSim/Analyses/Test/testOutputReporter.cpp @@ -197,16 +197,9 @@ void simulateMuscle( state.setTime(initialTime); manager.initialize(state); - cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl; - - // Start timing the simulation - const clock_t start = clock(); // simulate state = manager.integrate(finalTime); - // how long did it take? - double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC; - //========================================================================== // 4. Print the results //========================================================================== @@ -221,13 +214,19 @@ void simulateMuscle( double val_t0 = tableD.getIndependentColumn()[0]; const SimTK::Real& val_ke0 = tableD.getRowAtIndex(0)[0]; - const Vec3& val_omega0 = tableV3.getRowAtIndex(01)[1]; + const Vec3& val_omega0 = tableV3.getRowAtIndex(0)[1]; const SimTK::SpatialVec& val_jrf0 = tableSV.getRowAtIndex(0)[1]; - ASSERT_EQUAL(t0, val_t0, SimTK::Eps); - ASSERT_EQUAL(ke0, val_ke0, SimTK::Eps); - ASSERT_EQUAL(ang_acc0, val_omega0, SimTK::Eps); - ASSERT_EQUAL(reaction0, val_jrf0, SimTK::Eps); + CHECK_THAT(t0, Catch::Matchers::WithinAbs(val_t0, SimTK::Eps)); + CHECK_THAT(ke0, Catch::Matchers::WithinAbs(val_ke0, SimTK::Eps)); + for (int i = 0; i < 3; ++i) { + CHECK_THAT(ang_acc0[i], + Catch::Matchers::WithinAbs(val_omega0[i], SimTK::Eps)); + for (int j = 0; j < 2; ++j) { + CHECK_THAT(reaction0[j][i], + Catch::Matchers::WithinAbs(val_jrf0[j][i], SimTK::Eps)); + } + } double val_tf = tableD.getIndependentColumn()[tableD.getNumRows() - 1]; const SimTK::Real& val_ke = tableD.getRowAtIndex(tableD.getNumRows() - 1)[0]; @@ -240,10 +239,16 @@ void simulateMuscle( auto ang_acc = ball->getAngularAccelerationInGround(state); auto reaction = slider->calcReactionOnChildExpressedInGround(state); - ASSERT_EQUAL(state.getTime(), val_tf, SimTK::Eps); - ASSERT_EQUAL(ke, val_ke, SimTK::Eps); - ASSERT_EQUAL(ang_acc, val_omega, SimTK::Eps); - ASSERT_EQUAL(reaction, val_jrf, SimTK::Eps); + CHECK_THAT(state.getTime(), Catch::Matchers::WithinAbs(val_tf, SimTK::Eps)); + CHECK_THAT(ke, Catch::Matchers::WithinAbs(val_ke, 1e-10)); + for (int i = 0; i < 3; ++i) { + CHECK_THAT(ang_acc[i], + Catch::Matchers::WithinAbs(val_omega[i], SimTK::Eps)); + for (int j = 0; j < 2; ++j) { + CHECK_THAT(reaction[j][i], + Catch::Matchers::WithinAbs(val_jrf[j][i], SimTK::Eps)); + } + } } TEST_CASE("Output Reporter") diff --git a/OpenSim/Common/APDMDataReader.cpp b/OpenSim/Common/APDMDataReader.cpp index d10eef0e03..73e60cbf4b 100644 --- a/OpenSim/Common/APDMDataReader.cpp +++ b/OpenSim/Common/APDMDataReader.cpp @@ -4,6 +4,7 @@ #include "FileAdapter.h" #include "TimeSeriesTable.h" #include "APDMDataReader.h" +#include "IO.h" namespace OpenSim { @@ -101,7 +102,7 @@ APDMDataReader::extendRead(const std::string& fileName) const { // Line 2 std::getline(in_stream, line); tokens = FileAdapter::tokenize(line, ","); - dataRate = std::stod(tokens[1]); + dataRate = OpenSim::IO::stod(tokens[1]); // Line 3, find columns for IMUs std::getline(in_stream, line); tokens = FileAdapter::tokenize(line, ","); @@ -155,20 +156,20 @@ APDMDataReader::extendRead(const std::string& fileName) const { for (int imu_index = 0; imu_index < n_imus; ++imu_index) { // parse gyro info from in_stream if (foundLinearAccelerationData) - accel_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[accIndex[imu_index]]), - std::stod(nextRow[accIndex[imu_index] + 1]), std::stod(nextRow[accIndex[imu_index] + 2])); + accel_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[accIndex[imu_index]]), + OpenSim::IO::stod(nextRow[accIndex[imu_index] + 1]), OpenSim::IO::stod(nextRow[accIndex[imu_index] + 2])); if (foundMagneticHeadingData) - magneto_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[magIndex[imu_index]]), - std::stod(nextRow[magIndex[imu_index] + 1]), std::stod(nextRow[magIndex[imu_index] + 2])); + magneto_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[magIndex[imu_index]]), + OpenSim::IO::stod(nextRow[magIndex[imu_index] + 1]), OpenSim::IO::stod(nextRow[magIndex[imu_index] + 2])); if (foundAngularVelocityData) - gyro_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[gyroIndex[imu_index]]), - std::stod(nextRow[gyroIndex[imu_index] + 1]), std::stod(nextRow[gyroIndex[imu_index] + 2])); + gyro_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[gyroIndex[imu_index]]), + OpenSim::IO::stod(nextRow[gyroIndex[imu_index] + 1]), OpenSim::IO::stod(nextRow[gyroIndex[imu_index] + 2])); // Create Quaternion from values in file, assume order in file W, X, Y, Z orientation_row_vector[imu_index] = - SimTK::Quaternion(std::stod(nextRow[orientationsIndex[imu_index]]), - std::stod(nextRow[orientationsIndex[imu_index] + 1]), - std::stod(nextRow[orientationsIndex[imu_index] + 2]), - std::stod(nextRow[orientationsIndex[imu_index] + 3])); + SimTK::Quaternion(OpenSim::IO::stod(nextRow[orientationsIndex[imu_index]]), + OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 1]), + OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 2]), + OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 3])); } // append to the tables times[rowNumber] = time; diff --git a/OpenSim/Common/CMakeLists.txt b/OpenSim/Common/CMakeLists.txt index ef0508c4b6..afabfd39de 100644 --- a/OpenSim/Common/CMakeLists.txt +++ b/OpenSim/Common/CMakeLists.txt @@ -16,7 +16,7 @@ OpenSimAddLibrary( KIT Common AUTHORS "Clay_Anderson-Ayman_Habib-Peter_Loan" # Clients of osimCommon need not link to ezc3d. - LINKLIBS PUBLIC ${Simbody_LIBRARIES} spdlog::spdlog + LINKLIBS PUBLIC ${Simbody_LIBRARIES} spdlog::spdlog osimLepton PRIVATE ${ezc3d_LIBRARY} INCLUDES ${INCLUDES} SOURCES ${SOURCES} diff --git a/OpenSim/Common/DelimFileAdapter.h b/OpenSim/Common/DelimFileAdapter.h index 1224ef2034..9414060f9d 100644 --- a/OpenSim/Common/DelimFileAdapter.h +++ b/OpenSim/Common/DelimFileAdapter.h @@ -437,7 +437,7 @@ DelimFileAdapter::extendRead(const std::string& fileName) const { } // Time is column 0. - timeVec.push_back(std::stod(row.front())); + timeVec.push_back(OpenSim::IO::stod(row.front())); row.erase(row.begin()); auto row_vector = readElems(row); @@ -482,7 +482,7 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, double) const { SimTK::RowVector_ elems{static_cast(tokens.size())}; for(auto i = 0u; i < tokens.size(); ++i) - elems[static_cast(i)] = std::stod(tokens[i]); + elems[static_cast(i)] = OpenSim::IO::stod(tokens[i]); return elems; } @@ -497,9 +497,9 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, OPENSIM_THROW_IF(comps.size() != 3, IncorrectNumTokens, "Expected 3x (multiple of 3) number of tokens."); - elems[i] = SimTK::UnitVec3{std::stod(comps[0]), - std::stod(comps[1]), - std::stod(comps[2])}; + elems[i] = SimTK::UnitVec3{OpenSim::IO::stod(comps[0]), + OpenSim::IO::stod(comps[1]), + OpenSim::IO::stod(comps[2])}; } return elems; @@ -515,10 +515,10 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, OPENSIM_THROW_IF(comps.size() != 4, IncorrectNumTokens, "Expected 4x (multiple of 4) number of tokens."); - elems[i] = SimTK::Quaternion{std::stod(comps[0]), - std::stod(comps[1]), - std::stod(comps[2]), - std::stod(comps[3])}; + elems[i] = SimTK::Quaternion{OpenSim::IO::stod(comps[0]), + OpenSim::IO::stod(comps[1]), + OpenSim::IO::stod(comps[2]), + OpenSim::IO::stod(comps[3])}; } return elems; @@ -534,12 +534,12 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, OPENSIM_THROW_IF(comps.size() != 6, IncorrectNumTokens, "Expected 6x (multiple of 6) number of tokens."); - elems[i] = SimTK::SpatialVec{{std::stod(comps[0]), - std::stod(comps[1]), - std::stod(comps[2])}, - {std::stod(comps[3]), - std::stod(comps[4]), - std::stod(comps[5])}}; + elems[i] = SimTK::SpatialVec{{OpenSim::IO::stod(comps[0]), + OpenSim::IO::stod(comps[1]), + OpenSim::IO::stod(comps[2])}, + {OpenSim::IO::stod(comps[3]), + OpenSim::IO::stod(comps[4]), + OpenSim::IO::stod(comps[5])}}; } return elems; @@ -559,7 +559,7 @@ DelimFileAdapter::readElems_impl(const std::vector& tokens, "x (multiple of " + std::to_string(M) + ") number of tokens."); for(int j = 0; j < M; ++j) { - elems[i][j] = std::stod(comps[j]); + elems[i][j] = OpenSim::IO::stod(comps[j]); } } diff --git a/OpenSim/Common/ExpressionBasedFunction.cpp b/OpenSim/Common/ExpressionBasedFunction.cpp new file mode 100644 index 0000000000..f92e4edefe --- /dev/null +++ b/OpenSim/Common/ExpressionBasedFunction.cpp @@ -0,0 +1,131 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ExpressionBasedFunction.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "ExpressionBasedFunction.h" + +#include +#include +#include +#include + +using namespace OpenSim; + +class SimTKExpressionBasedFunction : public SimTK::Function { +public: + SimTKExpressionBasedFunction(const std::string& expression, + const std::vector& variables) : + m_expression(expression), m_variables(variables) { + + // Check that the variable names are unique. + std::set uniqueVariables; + for (const auto& variable : m_variables) { + if (!uniqueVariables.insert(variable).second) { + OPENSIM_THROW(Exception, + fmt::format("Variable '{}' is defined more than once.", + variable)); + } + } + + // Create the expression programs for the value and its derivatives. + Lepton::ParsedExpression parsedExpression = + Lepton::Parser::parse(m_expression).optimize(); + m_valueProgram = parsedExpression.createProgram(); + + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + Lepton::ParsedExpression diffExpression = + parsedExpression.differentiate(m_variables[i]).optimize(); + m_derivativePrograms.push_back(diffExpression.createProgram()); + } + + try { + std::map vars; + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + vars[m_variables[i]] = 0; + } + m_valueProgram.evaluate(vars); + + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + m_derivativePrograms[i].evaluate(vars); + } + } catch (Lepton::Exception& ex) { + std::string msg = ex.what(); + if (msg.compare(0, 30, "No value specified for variable")) { + std::string undefinedVar = msg.substr(32, msg.size() - 32); + OPENSIM_THROW(Exception, + fmt::format("Variable '{}' is not defined. Use " + "setVariables() to explicitly define this variable. " + "Or, remove it from the expression.", undefinedVar)); + } else { + OPENSIM_THROW(Exception, "Lepton parsing error: {}", msg); + } + } + } + + SimTK::Real calcValue(const SimTK::Vector& x) const override { + OPENSIM_ASSERT(x.size() == static_cast(m_variables.size())); + std::map vars; + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + vars[m_variables[i]] = x[i]; + } + return m_valueProgram.evaluate(vars); + } + + SimTK::Real calcDerivative(const SimTK::Array_& derivComponents, + const SimTK::Vector& x) const override { + OPENSIM_ASSERT(x.size() == static_cast(m_variables.size())); + OPENSIM_ASSERT(derivComponents.size() == 1); + if (derivComponents[0] < static_cast(m_variables.size())) { + std::map vars; + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + vars[m_variables[i]] = x[i]; + } + return m_derivativePrograms[derivComponents[0]].evaluate(vars); + } + return 0.0; + } + + int getArgumentSize() const override { + return static_cast(m_variables.size()); + } + int getMaxDerivativeOrder() const override { return 1; } + SimTKExpressionBasedFunction* clone() const override { + return new SimTKExpressionBasedFunction(*this); + } + +private: + std::string m_expression; + std::vector m_variables; + Lepton::ExpressionProgram m_valueProgram; + std::vector m_derivativePrograms; +}; + +SimTK::Function* ExpressionBasedFunction::createSimTKFunction() const { + OPENSIM_THROW_IF_FRMOBJ(get_expression().empty(), Exception, + "The expression has not been set. Use setExpression().") + + std::vector variables; + for (int i = 0; i < getProperty_variables().size(); ++i) { + variables.push_back(get_variables(i)); + } + return new SimTKExpressionBasedFunction(get_expression(), variables); +} \ No newline at end of file diff --git a/OpenSim/Common/ExpressionBasedFunction.h b/OpenSim/Common/ExpressionBasedFunction.h new file mode 100644 index 0000000000..b05d2dc70d --- /dev/null +++ b/OpenSim/Common/ExpressionBasedFunction.h @@ -0,0 +1,134 @@ +#ifndef OPENSIM_EXPRESSION_BASED_FUNCTION_H_ +#define OPENSIM_EXPRESSION_BASED_FUNCTION_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: ExpressionBasedFunction.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "osimCommonDLL.h" +#include "Function.h" +namespace OpenSim { + +/** + * A function based on a user-defined mathematical expression. + * + * This class allows users to define a function based on a mathematical + * expression (e.g., "x*sqrt(y-8)"). The expression can be a function of any + * number of independent variables. The expression is parsed and evaluated using + * the Lepton library. + * + * Set the expression using setExpression(). Any variables used in the + * expression must be explicitly defined using setVariables(). This + * implementation allows computation of first-order derivatives only. + * + * # Creating Expressions + * + * Expressions can contain variables, constants, operations, parentheses, commas, + * spaces, and scientific "e" notation. The full list of supported operations is: + * sqrt, exp, log, sin, cos, sec, csc, tan, cot, asin, acos, atan, sinh, cosh, + * tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, +, -, *, /, + * and ^. + */ +class OSIMCOMMON_API ExpressionBasedFunction : public Function { + OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedFunction, Function); + +public: +//============================================================================== +// PROPERTIES +//============================================================================== + OpenSim_DECLARE_PROPERTY(expression, std::string, + "The mathematical expression defining this Function."); + OpenSim_DECLARE_LIST_PROPERTY(variables, std::string, + "The independent variables used by this Function's expression. " + "In XML, variable names should be space-separated."); + +//============================================================================== +// METHODS +//============================================================================== + + /** Default constructor. */ + ExpressionBasedFunction() { constructProperties(); } + + /** Convenience constructor. + * + * @param expression The expression that defines this Function. + * @param variables The independent variable names of this expression. + */ + ExpressionBasedFunction(std::string expression, + const std::vector& variables) { + constructProperties(); + set_expression(std::move(expression)); + setVariables(variables); + } + + /** + * The mathematical expression that defines this Function. The expression + * should be a function of the variables defined via setVariables(). + * + * @note The expression cannot contain any whitespace characters. + */ + void setExpression(std::string expression) { + set_expression(std::move(expression)); + } + /// @copydoc setExpression() + const std::string& getExpression() const { + return get_expression(); + } + + /** + * The independent variable names of this expression. The variables names + * should be unique and should be comprised of alphabetic characters or any + * characters not reserved by Lepton (i.e., +, -, *, /, and ^). Variable + * names can contain numbers as long they do not come first in the name + * (e.g., "var0"). The input vector passed to calcValue() and + * calcDerivative() should be in the same order as the variables defined + * here. + */ + void setVariables(const std::vector& variables) { + for (const auto& var : variables) { + append_variables(var); + } + } + /// @copydoc setVariables() + std::vector getVariables() const { + std::vector variables; + for (int i = 0; i < getProperty_variables().size(); ++i) { + variables.push_back(get_variables(i)); + } + return variables; + } + + /** + * Return a pointer to a SimTK::Function object that implements this + * function. + */ + SimTK::Function* createSimTKFunction() const override; + +private: + void constructProperties() { + constructProperty_expression(""); + constructProperty_variables(); + } +}; + +} // namespace OpenSim + +#endif // OPENSIM_EXPRESSION_BASED_FUNCTION_H_ \ No newline at end of file diff --git a/OpenSim/Common/IO.cpp b/OpenSim/Common/IO.cpp index 66828b49b1..59fc759ee0 100644 --- a/OpenSim/Common/IO.cpp +++ b/OpenSim/Common/IO.cpp @@ -31,8 +31,10 @@ #include "Logger.h" #include +#include #include #include +#include #include #if defined(__linux__) || defined(__APPLE__) #include @@ -58,12 +60,28 @@ using namespace OpenSim; using namespace std; // STATICS -bool IO::_Scientific = false; -bool IO::_GFormatForDoubleOutput = false; -int IO::_Pad = 8; -int IO::_Precision = 8; -char IO::_DoubleFormat[] = "%16.8lf"; -bool IO::_PrintOfflineDocuments = true; +namespace +{ + constexpr int IO_DBLFMTLEN = 256; + + /** Specifies whether number output is in scientific or float format. */ + bool _Scientific = false; + + /** Specifies whether number output is in %g format or not. */ + bool _GFormatForDoubleOutput = false; + + /** Specifies number of digits of padding in number output. */ + int _Pad = 8; + + /** Specifies the precision of number output. */ + int _Precision = 8; + + /** The output format string. */ + char _DoubleFormat[IO_DBLFMTLEN] = "%16.8lf"; + + /** Whether offline documents should also be printed when Object::print is called. */ + bool _PrintOfflineDocuments = true; +} //============================================================================= @@ -474,6 +492,26 @@ OpenOutputFile(const string &aFileName,ios_base::openmode mode) return(fs); } + +double IO:: +stod(const std::string& __str, std::size_t* __idx) +{ + std::istringstream iss(__str); + + // Always parse numbers with "C" locale, which uses a period character + // for the decimal place. Otherwise, Finns, Dutch, and other locales + // that use comma characters as a decimal place will encounter parsing + // issues (#3943, #3924). + iss.imbue(std::locale::classic()); + + double result; + iss >> result; + if (iss.fail()) { + result = std::numeric_limits::quiet_NaN(); + log_warn("Encountered non-numeric string value: {} ; parsed value:{}",__str, result); + } + return result; +} //_____________________________________________________________________________ /** * Create a directory. Potentially platform dependent. diff --git a/OpenSim/Common/IO.h b/OpenSim/Common/IO.h index 2b403b959d..24c6306d12 100644 --- a/OpenSim/Common/IO.h +++ b/OpenSim/Common/IO.h @@ -35,7 +35,6 @@ // DEFINES constexpr int IO_STRLEN = 2048; -constexpr int IO_DBLFMTLEN = 256; namespace OpenSim { @@ -53,19 +52,6 @@ class OSIMCOMMON_API IO { // DATA //============================================================================= private: - // NUMBER OUTPUT - /** Specifies whether number output is in scientific or float format. */ - static bool _Scientific; - /** Specifies whether number output is in %g format or not. */ - static bool _GFormatForDoubleOutput; - /** Specifies number of digits of padding in number output. */ - static int _Pad; - /** Specifies the precision of number output. */ - static int _Precision; - /** The output format string. */ - static char _DoubleFormat[IO_DBLFMTLEN]; - /** Whether offline documents should also be printed when Object::print is called. */ - static bool _PrintOfflineDocuments; //============================================================================= @@ -103,6 +89,7 @@ class OSIMCOMMON_API IO { static FILE* OpenFile(const std::string &aFileName,const std::string &aMode); static std::ifstream* OpenInputFile(const std::string &aFileName,std::ios_base::openmode mode=std::ios_base::in); static std::ofstream* OpenOutputFile(const std::string &aFileName,std::ios_base::openmode mode=std::ios_base::out); + static double stod(const std::string& __str, std::size_t* __idx = 0); #endif // Directory management static int makeDir(const std::string &aDirName); diff --git a/OpenSim/Common/RegisterTypes_osimCommon.cpp b/OpenSim/Common/RegisterTypes_osimCommon.cpp index 2be4dec912..114a4fde20 100644 --- a/OpenSim/Common/RegisterTypes_osimCommon.cpp +++ b/OpenSim/Common/RegisterTypes_osimCommon.cpp @@ -41,6 +41,7 @@ #include "MultiplierFunction.h" #include "PolynomialFunction.h" #include "MultivariatePolynomialFunction.h" +#include "ExpressionBasedFunction.h" #include "SignalGenerator.h" @@ -88,6 +89,7 @@ OSIMCOMMON_API void RegisterTypes_osimCommon() Object::registerType( MultiplierFunction() ); Object::registerType( PolynomialFunction() ); Object::registerType( MultivariatePolynomialFunction() ); + Object::registerType( ExpressionBasedFunction() ); Object::registerType( SignalGenerator() ); diff --git a/OpenSim/Common/TRCFileAdapter.cpp b/OpenSim/Common/TRCFileAdapter.cpp index 39747fc43b..54ba7a8bd6 100644 --- a/OpenSim/Common/TRCFileAdapter.cpp +++ b/OpenSim/Common/TRCFileAdapter.cpp @@ -193,15 +193,15 @@ TRCFileAdapter::extendRead(const std::string& fileName) const { //only if each component is specified read process as a Vec3 if ( !(row.at(c).empty() || row.at(c + 1).empty() || row.at(c + 2).empty()) ) { - row_vector[ind] = SimTK::Vec3{ std::stod(row.at(c)), - std::stod(row.at(c + 1)), - std::stod(row.at(c + 2)) }; + row_vector[ind] = SimTK::Vec3{ OpenSim::IO::stod(row.at(c)), + OpenSim::IO::stod(row.at(c + 1)), + OpenSim::IO::stod(row.at(c + 2)) }; } // otherwise the value will remain NaN (default) ++ind; } markerData[rowNumber] = row_vector; // Column 1 is time. - times[rowNumber] = std::stod(row.at(1)); + times[rowNumber] = OpenSim::IO::stod(row.at(1)); rowNumber++; if (rowNumber== last_size) { // resize all Data/Matrices, double the size while keeping data diff --git a/OpenSim/Common/Test/testComponentInterface.cpp b/OpenSim/Common/Test/testComponentInterface.cpp index 984760a491..74db2cc5fe 100644 --- a/OpenSim/Common/Test/testComponentInterface.cpp +++ b/OpenSim/Common/Test/testComponentInterface.cpp @@ -390,7 +390,7 @@ class Bar : public Component { // and to access a dv that is not type double. // The following calls put the mo and dv into the maps used to contain // all mo's and dv's exposed in OpenSim. When Stage::Topology is - // realized, they will allocated in class Bar's override of + // realized, they will be allocated in class Bar's override of // extendRealizeTopology(). See below. bool allocate = false; int maxFlagValue = 1; @@ -401,6 +401,8 @@ class Bar : public Component { // Manually allocate and update the index and subsystem for // a discrete variable and a modeling option as though they were // natively allocated in Simbody and brought into OpenSim. + // Note, as of May 2024, this is also what one would need to do in order + // to add a discrete variable that is a type other than double. void extendRealizeTopology(SimTK::State& state) const override { Super::extendRealizeTopology(state); @@ -2057,7 +2059,7 @@ TEST_CASE("Component Interface State Trajectories") } // Create a new state trajectory (as though deserializing) - // newTraj must be must the expected size before any set calls. + // newTraj must be the expected size before any set calls. SimTK::Array_ newTraj; for (int i = 0; i < nsteps; ++i) newTraj.emplace_back(s); // state variables diff --git a/OpenSim/Common/Test/testDataTable.cpp b/OpenSim/Common/Test/testDataTable.cpp index 3dfdc1fc80..7086ffcf3a 100644 --- a/OpenSim/Common/Test/testDataTable.cpp +++ b/OpenSim/Common/Test/testDataTable.cpp @@ -243,6 +243,9 @@ TEST_CASE("DataTable") { ASSERT((static_cast (DataTable_{})). numComponentsPerElement() == 6); + ASSERT((static_cast + (DataTable_{})). + numComponentsPerElement() == 9); { std::cout << "Test DataTable flattening constructor for Vec3." @@ -371,6 +374,26 @@ TEST_CASE("DataTable") { ASSERT(tableDouble.getNumColumns() == 18); std::cout << tableDouble << std::endl; + + std::cout << "Test DataTable flattening constructor for Rotation." + << std::endl; + DataTable_ tableRotation{}; + tableRotation.setColumnLabels({"col0", "col1"}); + tableRotation.appendRow(0.1, {Rotation(0.1, UnitVec3(1, 0, 0)), + Rotation(0.2, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.2, {Rotation(0.2, UnitVec3(0, 0, 1)), + Rotation(0.1, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.3, {Rotation(0.3, UnitVec3(0, 1, 0)), + Rotation(0.2, UnitVec3(1, 0, 0))}); + + std::cout << tableRotation << std::endl; + + tableDouble = tableRotation; + ASSERT(tableDouble.getColumnLabels().size() == 18); + ASSERT(tableDouble.getNumRows() == 3); + ASSERT(tableDouble.getNumColumns() == 18); + + std::cout << tableDouble << std::endl; } { std::cout << "Test TimeSeriesTable flattening constructor for Vec3" @@ -525,6 +548,43 @@ TEST_CASE("DataTable") { ASSERT(tableDouble.getNumColumns() == 18); std::cout << tableDouble << std::endl; + + std::cout << "Test TimeSeriesTable flattening constructor for " + "Rotation" << std::endl; + DataTable_ tableRotation{}; + tableRotation.setColumnLabels({"col0", "col1"}); + tableRotation.appendRow(0.1, {Rotation(0.1, UnitVec3(1, 0, 0)), + Rotation(0.2, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.2, {Rotation(0.2, UnitVec3(0, 0, 1)), + Rotation(0.1, UnitVec3(0, 1, 0))}); + tableRotation.appendRow(0.3, {Rotation(0.3, UnitVec3(0, 1, 0)), + Rotation(0.2, UnitVec3(1, 0, 0))}); + + // TODO: RowVector_ is not supported. + // const auto& avgRowRot = tableRotation.averageRow(0.1, 0.2); + // for(int i = 0; i < 3; ++i) { + // OPENSIM_THROW_IF(std::abs(avgRowRot[0][0][i] - 2) > 1e-8/*eps*/, + // OpenSim::Exception, + // "Test failed: averageRow() failed."); + // } + + // const auto& nearRowRot = tableRotation.getNearestRow(0.29); + // for(int i = 0; i < 3; ++i) + // ASSERT(nearRowRot[0][0][i] == 2); + + // tableRotation.updNearestRow(0.29) += Rotation(0.2, UnitVec3(0, 1, 0)); + // tableRotation.updNearestRow(0.29) -= Rotation(0.2, UnitVec3(0, 1, 0)); + // for(int i = 0; i < 3; ++i) + // ASSERT(nearRowRot[0][0][i] == 2); + + std::cout << tableRotation << std::endl; + + tableDouble = tableRotation; + ASSERT(tableDouble.getColumnLabels().size() == 18); + ASSERT(tableDouble.getNumRows() == 3); + ASSERT(tableDouble.getNumColumns() == 18); + + std::cout << tableDouble << std::endl; } { std::cout << "Test DataTable packing." << std::endl; @@ -601,6 +661,32 @@ TEST_CASE("DataTable") { ASSERT(tableSVec.getTableMetaData("string") == "string"); ASSERT(tableSVec.getTableMetaData("int") == 10); std::cout << tableSVec << std::endl; + + std::cout << "Test DataTable packing for Rotation" << std::endl; + DataTable_ table{}; + table.setColumnLabels({"col0_0", "col0_1", "col0_2", + "col0_3", "col0_4", "col0_5", + "col0_6", "col0_7", "col0_8", + "col1_0", "col1_1", "col1_2", + "col1_3", "col1_4", "col1_5", + "col1_6", "col1_7", "col1_8"}); + table.appendRow(1, RowVector(18, 1)); + table.appendRow(2, RowVector(18, 2)); + table.appendRow(3, RowVector(18, 3)); + table.addTableMetaData("string", std::string{"string"}); + table.addTableMetaData("int", 10); + ASSERT(table.getColumnLabels().size() == 18); + ASSERT(table.getNumRows() == 3); + ASSERT(table.getNumColumns() == 18); + + auto tableRot = table.pack(); + expLabels = {"col0", "col1"}; + ASSERT(tableRot.getColumnLabels() == expLabels); + ASSERT(tableRot.getNumRows() == 3); + ASSERT(tableRot.getNumColumns() == 2); + ASSERT(tableRot.getTableMetaData("string") == "string"); + ASSERT(tableRot.getTableMetaData("int") == 10); + std::cout << tableRot << std::endl; } { std::cout << "Test TimeSeriesTable packing." << std::endl; @@ -692,6 +778,33 @@ TEST_CASE("DataTable") { ASSERT(tableSVec.getTableMetaData("string") == "string"); ASSERT(tableSVec.getTableMetaData("int") == 10); std::cout << tableSVec << std::endl; + + std::cout << "Test TimeSeriesTable packing for Rotation" << std::endl; + TimeSeriesTable_ table{}; + table.setColumnLabels({"col0_0", "col0_1", "col0_2", + "col0_3", "col0_4", "col0_5", + "col0_6", "col0_7", "col0_8", + "col1_0", "col1_1", "col1_2", + "col1_3", "col1_4", "col1_5", + "col1_6", "col1_7", "col1_8"}); + table.appendRow(1, RowVector(18, 1)); + table.appendRow(2, RowVector(18, 2)); + table.appendRow(3, RowVector(18, 3)); + table.addTableMetaData("string", std::string{"string"}); + table.addTableMetaData("int", 10); + ASSERT(table.getColumnLabels().size() == 18); + ASSERT(table.getNumRows() == 3); + ASSERT(table.getNumColumns() == 18); + + TimeSeriesTable_ tableRot = + table.pack(); + expLabels = {"col0", "col1"}; + ASSERT(tableRot.getColumnLabels() == expLabels); + ASSERT(tableRot.getNumRows() == 3); + ASSERT(tableRot.getNumColumns() == 2); + ASSERT(tableRot.getTableMetaData("string") == "string"); + ASSERT(tableRot.getTableMetaData("int") == 10); + std::cout << tableRot << std::endl; } { diff --git a/OpenSim/Common/Test/testFunctions.cpp b/OpenSim/Common/Test/testFunctions.cpp index 69fb85d81f..d854c914c9 100644 --- a/OpenSim/Common/Test/testFunctions.cpp +++ b/OpenSim/Common/Test/testFunctions.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include @@ -260,3 +261,92 @@ TEST_CASE("solveBisection()") { REQUIRE_THROWS_AS(solveBisection(parabola, -5, 5), OpenSim::Exception); } } + +TEST_CASE("ExpressionBasedFunction") { + const SimTK::Real x = SimTK::Test::randReal(); + const SimTK::Real y = SimTK::Test::randReal(); + const SimTK::Real z = SimTK::Test::randReal(); + + SECTION("Square-root function") { + ExpressionBasedFunction f("sqrt(x)", {"x"}); + REQUIRE_THAT(f.calcValue(createVector({x})), + Catch::Matchers::WithinAbs(std::sqrt(x), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x})), + Catch::Matchers::WithinAbs(0.5 / std::sqrt(x), 1e-10)); + } + + SECTION("Exponential function") { + ExpressionBasedFunction f("exp(x)", {"x"}); + REQUIRE_THAT(f.calcValue(createVector({x})), + Catch::Matchers::WithinAbs(std::exp(x), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x})), + Catch::Matchers::WithinAbs(std::exp(x), 1e-10)); + } + + SECTION("Multivariate function") { + ExpressionBasedFunction f("2*x^3 + 3*y*z^2", {"x", "y", "z"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(2*x*x*x + 3*y*z*z, 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(6*x*x, 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(3*z*z, 1e-10)); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(6*y*z, 1e-10)); + } + + + SECTION("Sinusoidal function") { + ExpressionBasedFunction f("x*sin(y*z^2)", {"x", "y", "z"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(x * std::sin(y*z*z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(std::sin(y*z*z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(x*z*z*std::cos(y*z*z), 1e-10)); + } + + SECTION("Undefined variable in expression") { + ExpressionBasedFunction f("x*y", {"x"}); + REQUIRE_THROWS_WITH(f.calcValue(createVector({x, y})), + Catch::Matchers::ContainsSubstring( + "Variable 'y' is not defined.")); + } + + SECTION("Extra variables should have zero derivative") { + ExpressionBasedFunction f("x*y", {"x", "y", "z"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(x*y, 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(y, 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(x, 1e-10)); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(0.0, 1e-10)); + } + + SECTION("Derivative of nonexistent variable") { + ExpressionBasedFunction f("x*y", {"x", "y"}); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y})), + Catch::Matchers::WithinAbs(0.0, 1e-10)); + } + + SECTION("Variable defined multiple times") { + ExpressionBasedFunction f("x", {"x", "x"}); + REQUIRE_THROWS_WITH(f.calcValue(createVector({x})), + Catch::Matchers::ContainsSubstring( + "Variable 'x' is defined more than once.")); + } + + SECTION("Non-alphabetic variable names") { + ExpressionBasedFunction f("@^2 + %*cos(&)", {"@", "%", "&"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(x*x + y*std::cos(z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(2*x, 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(std::cos(z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(-y*std::sin(z), 1e-10)); + } +} \ No newline at end of file diff --git a/OpenSim/Common/Test/testSTOFileAdapter.cpp b/OpenSim/Common/Test/testSTOFileAdapter.cpp index 600058d1cc..6f3cd746d2 100644 --- a/OpenSim/Common/Test/testSTOFileAdapter.cpp +++ b/OpenSim/Common/Test/testSTOFileAdapter.cpp @@ -22,6 +22,7 @@ #include "OpenSim/Common/Adapters.h" #include "OpenSim/Common/CommonUtilities.h" +#include "OpenSim/Common/IO.h" #include #include #include @@ -140,8 +141,8 @@ void compareFiles(const std::string& filenameA, double d_tokenA{}; double d_tokenB{}; try { - d_tokenA = std::stod(tokenA); - d_tokenB = std::stod(tokenB); + d_tokenA = OpenSim::IO::stod(tokenA); + d_tokenB = OpenSim::IO::stod(tokenB); } catch(std::invalid_argument&) { testFailed(filenameA, tokenA, tokenB); } diff --git a/OpenSim/Common/Test/testTRCFileAdapter.cpp b/OpenSim/Common/Test/testTRCFileAdapter.cpp index 555d4c7acd..5512f8d917 100644 --- a/OpenSim/Common/Test/testTRCFileAdapter.cpp +++ b/OpenSim/Common/Test/testTRCFileAdapter.cpp @@ -107,8 +107,8 @@ void compareFiles(const std::string& filenameA, double d_tokenA{}; double d_tokenB{}; try { - d_tokenA = std::stod(tokenA); - d_tokenB = std::stod(tokenB); + d_tokenA = OpenSim::IO::stod(tokenA); + d_tokenB = OpenSim::IO::stod(tokenB); } catch (std::invalid_argument&) { testFailed(filenameA, tokenA, tokenB); diff --git a/OpenSim/Common/XsensDataReader.cpp b/OpenSim/Common/XsensDataReader.cpp index 5d5e1c9896..3287430eba 100644 --- a/OpenSim/Common/XsensDataReader.cpp +++ b/OpenSim/Common/XsensDataReader.cpp @@ -82,7 +82,7 @@ XsensDataReader::extendRead(const std::string& folderName) const { std::map::iterator it = headersKeyValuePairs.find("Update Rate"); if (it != headersKeyValuePairs.end()) - dataRate = std::stod(it->second); + dataRate = OpenSim::IO::stod(it->second); else dataRate = 40.0; // Need confirmation from XSens as later files don't specify rate // internally keep track of what data was found in input files @@ -122,20 +122,20 @@ XsensDataReader::extendRead(const std::string& folderName) const { break; } if (foundLinearAccelerationData) - accel_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[accIndex]), - std::stod(nextRow[accIndex + 1]), std::stod(nextRow[accIndex + 2])); + accel_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[accIndex]), + OpenSim::IO::stod(nextRow[accIndex + 1]), OpenSim::IO::stod(nextRow[accIndex + 2])); if (foundMagneticHeadingData) - magneto_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[magIndex]), - std::stod(nextRow[magIndex + 1]), std::stod(nextRow[magIndex + 2])); + magneto_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[magIndex]), + OpenSim::IO::stod(nextRow[magIndex + 1]), OpenSim::IO::stod(nextRow[magIndex + 2])); if (foundAngularVelocityData) - gyro_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[gyroIndex]), - std::stod(nextRow[gyroIndex + 1]), std::stod(nextRow[gyroIndex + 2])); + gyro_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[gyroIndex]), + OpenSim::IO::stod(nextRow[gyroIndex + 1]), OpenSim::IO::stod(nextRow[gyroIndex + 2])); // Create Mat33 then convert into Quaternion SimTK::Mat33 imu_matrix{ SimTK::NaN }; int matrix_entry_index = 0; for (int mcol = 0; mcol < 3; mcol++) { for (int mrow = 0; mrow < 3; mrow++) { - imu_matrix[mrow][mcol] = std::stod(nextRow[rotationsIndex + matrix_entry_index]); + imu_matrix[mrow][mcol] = OpenSim::IO::stod(nextRow[rotationsIndex + matrix_entry_index]); matrix_entry_index++; } } diff --git a/OpenSim/Common/osimCommon.h b/OpenSim/Common/osimCommon.h index 23cd153237..19adf1e7bb 100644 --- a/OpenSim/Common/osimCommon.h +++ b/OpenSim/Common/osimCommon.h @@ -29,6 +29,7 @@ #include "CommonUtilities.h" #include "Constant.h" #include "DataTable.h" +#include "ExpressionBasedFunction.h" #include "FunctionSet.h" #include "GCVSpline.h" #include "GCVSplineSet.h" diff --git a/OpenSim/Examples/BuildDynamicWalker/CMakeLists.txt b/OpenSim/Examples/BuildDynamicWalker/CMakeLists.txt index 56bee4fc63..5c6e773294 100644 --- a/OpenSim/Examples/BuildDynamicWalker/CMakeLists.txt +++ b/OpenSim/Examples/BuildDynamicWalker/CMakeLists.txt @@ -3,8 +3,8 @@ cmake_minimum_required(VERSION 3.2) project(BuildDynamicWalker) -# OpenSim requires a compiler that supports C++11. -set(CMAKE_CXX_STANDARD 11) +# OpenSim requires a compiler that supports c++14. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find the OpenSim libraries and header files. diff --git a/OpenSim/Examples/ControllerExample/CMakeLists.txt b/OpenSim/Examples/ControllerExample/CMakeLists.txt index 95bfbff393..53e66540c6 100644 --- a/OpenSim/Examples/ControllerExample/CMakeLists.txt +++ b/OpenSim/Examples/ControllerExample/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET exampleController CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/CustomActuatorExample/CMakeLists.txt b/OpenSim/Examples/CustomActuatorExample/CMakeLists.txt index 16a7250b99..c762781479 100644 --- a/OpenSim/Examples/CustomActuatorExample/CMakeLists.txt +++ b/OpenSim/Examples/CustomActuatorExample/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET exampleCustomActuator CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h b/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h index cf6ef65f6e..7b3f391336 100644 --- a/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h +++ b/OpenSim/Examples/CustomActuatorExample/ControllableSpring.h @@ -25,6 +25,8 @@ #include "PistonActuator.h" +#include + //============================================================================= //============================================================================= /** @@ -79,18 +81,20 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControllableSpring, PistonActuator); void setRestLength(double aLength) { set_rest_length(aLength); }; double getRestLength() const { return get_rest_length(); }; - //-------------------------------------------------------------------------- - // COMPUTATIONS - //-------------------------------------------------------------------------- - - /* The computeForce method is the meat of this simple actuator example. It - * computes the direction and distance between the two application points. - * It then uses the difference between its current length and rest length - * to determine the force magnitude, then applies the force at the - * application points, in the direction between them. */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override +private: + /** + * Implements the `ForceProducer` API, which is the "meat" of this example. + * + * The implementation: + * + * - Computes the direction and distance between the two application points. + * - Then uses the difference between its current length and rest length to + * determine the force magnitude + * - Then emits (produces into the `ForceConsumer`) the force as a point + * force at the application points that points in the direction between + * the two points. + */ + void implProduceForces(const SimTK::State& s, ForceConsumer& forceConsumer) const override { const PhysicalFrame& frameA = getFrameA(); const PhysicalFrame& frameB = getFrameB(); @@ -131,9 +135,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControllableSpring, PistonActuator); setActuation(s, forceMagnitude); SimTK::Vec3 force = forceMagnitude*direction; - // Apply equal and opposite forces to the bodies. - applyForceToPoint(s, frameA, pointA, force, bodyForces); - applyForceToPoint(s, frameB, pointB, -force, bodyForces); + // Produce equal and opposite point forces + forceConsumer.consumePointForce(s, frameA, pointA, force); + forceConsumer.consumePointForce(s, frameB, pointB, -force); } //============================================================================= diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp index 566d9406ac..33774da836 100644 --- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp +++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.cpp @@ -21,10 +21,8 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= #include "PistonActuator.h" + #include using namespace OpenSim; @@ -103,13 +101,9 @@ double PistonActuator::getStress( const SimTK::State& s) const } -//============================================================================= -// FORCE INTERFACE -//============================================================================= -void PistonActuator::computeForce( +void PistonActuator::implProduceForces( const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const + ForceConsumer& forceConsumer) const { const PhysicalFrame& frameA = getFrameA(); const PhysicalFrame& frameB = getFrameB(); @@ -128,9 +122,9 @@ void PistonActuator::computeForce( setActuation(s, forceMagnitude); SimTK::Vec3 force = forceMagnitude * calcDirectionBAInGround(s); - // Apply equal and opposite forces to the bodies. - applyForceToPoint(s, frameA, pointA, force, bodyForces); - applyForceToPoint(s, frameB, pointB, -force, bodyForces); + // Produce equal and opposite forces. + forceConsumer.consumePointForce(s, frameA, pointA, force); + forceConsumer.consumePointForce(s, frameB, pointB, -force); } SimTK::UnitVec3 PistonActuator::calcDirectionBAInGround( diff --git a/OpenSim/Examples/CustomActuatorExample/PistonActuator.h b/OpenSim/Examples/CustomActuatorExample/PistonActuator.h index 1f8401ce24..91df4b8311 100644 --- a/OpenSim/Examples/CustomActuatorExample/PistonActuator.h +++ b/OpenSim/Examples/CustomActuatorExample/PistonActuator.h @@ -119,13 +119,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PistonActuator, ScalarActuator); /** This is the absolute value of the force generated by this actuator * divided by its optimal force. */ double getStress(const SimTK::State& s) const override; - //-------------------------------------------------------------------------- - // FORCE INTERFACE - //-------------------------------------------------------------------------- - /** Apply the actuator force to frameA and frameB. */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; //-------------------------------------------------------------------------- // ACTUATOR INTERFACE @@ -141,6 +134,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PistonActuator, ScalarActuator); double getSpeed(const SimTK::State& s) const override; private: + /** + * Implements the `ForceProducer` interface by emitting point forces on + * frameA and frameB. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + /** Compute the direction of the actuator force in ground frame. */ SimTK::UnitVec3 calcDirectionBAInGround(const SimTK::State& s) const; diff --git a/OpenSim/Examples/ExampleCMakeListsToInstall.txt.in b/OpenSim/Examples/ExampleCMakeListsToInstall.txt.in index 3f67f7c0e2..4703bfb39a 100644 --- a/OpenSim/Examples/ExampleCMakeListsToInstall.txt.in +++ b/OpenSim/Examples/ExampleCMakeListsToInstall.txt.in @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.2) project(OpenSim_@_example_name@) -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) find_package(OpenSim REQUIRED HINTS "${CMAKE_SOURCE_DIR}/@_opensim_install_hint@") diff --git a/OpenSim/Examples/ExampleLuxoMuscle/CMakeLists.txt b/OpenSim/Examples/ExampleLuxoMuscle/CMakeLists.txt index 1ee639cf2c..b31966460a 100644 --- a/OpenSim/Examples/ExampleLuxoMuscle/CMakeLists.txt +++ b/OpenSim/Examples/ExampleLuxoMuscle/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET exampleLuxoMuscle CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/ExampleMain/CMakeLists.txt b/OpenSim/Examples/ExampleMain/CMakeLists.txt index 5dee7c2009..525037af1a 100644 --- a/OpenSim/Examples/ExampleMain/CMakeLists.txt +++ b/OpenSim/Examples/ExampleMain/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET exampleMain CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp b/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp index 9b91023ebb..d8e8713455 100644 --- a/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp +++ b/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp @@ -299,18 +299,15 @@ void muscleDrivenJointMomentTracking() { // Ignore coordinates that are locked, prescribed, or coupled to other // coordinates via CoordinateCouplerConstraints (true by default). jointMomentTracking->setIgnoreConstrainedCoordinates(true); - for (const auto& coordinate : model.getComponentList()) { - const auto& coordName = coordinate.getName(); - // Don't track generalized forces associated with pelvis residuals. - if (coordName.find("pelvis") != std::string::npos) { - jointMomentTracking->setWeightForGeneralizedForce(coordName, 0); - } - // Encourage better tracking of the ankle joint moments. - if (coordName.find("ankle") != std::string::npos) { - jointMomentTracking->setWeightForGeneralizedForce(coordName, 100); - } - } + // Do not track generalized forces associated with pelvis residuals. + jointMomentTracking->setWeightForGeneralizedForcePattern(".*pelvis.*", 0); + + // Encourage better tracking of the ankle joint moments. + jointMomentTracking->setWeightForGeneralizedForce( + "ankle_angle_r_moment", 100); + jointMomentTracking->setWeightForGeneralizedForce( + "ankle_angle_l_moment", 100); // Update the solver problem and tolerances. auto& solver = study.updSolver(); diff --git a/OpenSim/Examples/MuscleExample/CMakeLists.txt b/OpenSim/Examples/MuscleExample/CMakeLists.txt index e13f3c89e0..d7b6d4ffe5 100644 --- a/OpenSim/Examples/MuscleExample/CMakeLists.txt +++ b/OpenSim/Examples/MuscleExample/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET exampleMuscle CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/OptimizationExample_Arm26/CMakeLists.txt b/OpenSim/Examples/OptimizationExample_Arm26/CMakeLists.txt index 7c261492d8..6f1754caa9 100644 --- a/OpenSim/Examples/OptimizationExample_Arm26/CMakeLists.txt +++ b/OpenSim/Examples/OptimizationExample_Arm26/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET optimizationExample CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/PluginExampleCMakeListsToInstall.txt.in b/OpenSim/Examples/PluginExampleCMakeListsToInstall.txt.in index 768c6b3d4e..e37083c2fc 100644 --- a/OpenSim/Examples/PluginExampleCMakeListsToInstall.txt.in +++ b/OpenSim/Examples/PluginExampleCMakeListsToInstall.txt.in @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.2) project(OpenSim_@_example_name@) -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) find_package(OpenSim REQUIRED HINTS "${CMAKE_SOURCE_DIR}/@_opensim_install_hint@") diff --git a/OpenSim/Examples/Plugins/AnalysisPluginExample/CMakeLists.txt b/OpenSim/Examples/Plugins/AnalysisPluginExample/CMakeLists.txt index 6de2f71990..1936ccb790 100644 --- a/OpenSim/Examples/Plugins/AnalysisPluginExample/CMakeLists.txt +++ b/OpenSim/Examples/Plugins/AnalysisPluginExample/CMakeLists.txt @@ -7,8 +7,8 @@ file(GLOB INCLUDE_FILES *.h) set(PLUGIN_NAME "osimPlugin" CACHE STRING "Name of shared library to create") -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(OpenSim REQUIRED PATHS "${OPENSIM_INSTALL_DIR}") diff --git a/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.cpp b/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.cpp index 1158bfd6c3..bdd6b8cd57 100644 --- a/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.cpp +++ b/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.cpp @@ -24,9 +24,11 @@ //============================================================================= // INCLUDES //============================================================================= +#include "BodyDragForce.h" + #include +#include #include -#include "BodyDragForce.h" //============================================================================= // STATICS @@ -42,7 +44,7 @@ using namespace OpenSim; /** * Default constructor */ -BodyDragForce::BodyDragForce() : Force() +BodyDragForce::BodyDragForce() { setNull(); constructProperties(); @@ -107,9 +109,9 @@ void BodyDragForce::connectToModel(Model& aModel) // COMPUTATION //============================================================================= -void BodyDragForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void BodyDragForce::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { if(!_model) return; // some minor error checking @@ -139,11 +141,11 @@ void BodyDragForce::computeForce(const SimTK::State& s, dragForceGround, aBody); - // Apply drag force to the body - // ------------------------------ - // applyForceToPoint requires the force application point to be in the inertial (ground) frame + // Produce drag force as a point force on the body + // ----------------------------------------------- + // `consumePointForce` requires the force application point to be in the inertial (ground) frame // and the force vector itself to be in the body frame - applyForceToPoint(s, aBody, bodyCoMPosGround, dragForceBody, bodyForces); + forceConsumer.consumePointForce(s, aBody, bodyCoMPosGround, dragForceBody); @@ -200,7 +202,7 @@ OpenSim::Array BodyDragForce::getRecordValues(const SimTK::State& s) con SimTK::Vec3 bodyCoMPosBody, bodyCoMPosGround, bodyCoMVelGround, bodyCoMVelGroundRaisedPower, dragForceGround, dragForceBody, oppVelSign; BodySet &bs = _model->updBodySet(); // get body set const Ground &ground = _model->getGround(); // get ground body - Body &aBody = bs.get(get_body_name()); // get the body to apply the force to + Body &aBody = bs.get(get_body_name()); // get the body in which the force is applied // get CoM position of body in the BODY coordinate system bodyCoMPosBody = aBody.getMassCenter(); diff --git a/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.h b/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.h index 0c8b09f910..a8d7a2d5fc 100644 --- a/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.h +++ b/OpenSim/Examples/Plugins/BodyDragExample/BodyDragForce.h @@ -27,9 +27,10 @@ // INCLUDES //============================================================================= // Headers define the various property types that OpenSim objects can read -#include #include "osimPluginDLL.h" -#include +#include + +#include //============================================================================= @@ -46,7 +47,7 @@ */ namespace OpenSim { -class OSIMPLUGIN_API BodyDragForce : public Force +class OSIMPLUGIN_API BodyDragForce : public ForceProducer { OpenSim_DECLARE_CONCRETE_OBJECT(BodyDragForce, Force); public: @@ -96,23 +97,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(BodyDragForce, Force); // Default Constructor BodyDragForce(); - - //-------------------------------------------------------------------------- - // COMPUTATION - //-------------------------------------------------------------------------- - /** Compute the bushing force contribution to the system and add in to appropriate - * bodyForce and/or system generalizedForce. The bushing force is [K]*dq + [D]*dqdot - * where, [K] is the spatial 6dof stiffness matrix between the two frames - dq is the deflection in body spatial coordinates with rotations in Euler angles - * [D] is the spatial 6dof damping matrix opposing the velocity between the frames - * dqdot is the relative spatial velocity of the two frames - * BodyDragForce implementation based SimTK::Force::LinearBushing - * developed and implemented by Michael Sherman. - */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - /** Potential energy is determined by the elastic energy storage of the bushing. In spatial terms, U = ~dq*[K]*dq, with K and dq defined above. */ double computePotentialEnergy(const SimTK::State& s) const override; @@ -135,6 +119,22 @@ OpenSim_DECLARE_CONCRETE_OBJECT(BodyDragForce, Force); private: + /** + * Implements the `ForceProducer` API by computing the bushing force and emitting it + * into the `ForceConsumer`. + * + * The bushing force is [K]*dq + [D]*dqdot where: + * + * - [K] is the spatial 6dof stiffness matrix between the two frames + * - dq is the deflection in body spatial coordinates with rotations in Euler angles + * - [D] is the spatial 6dof damping matrix opposing the velocity between the frames + * - dqdot is the relative spatial velocity of the two frames + * + * This `BodyDragForce` implementation is based on `SimTK::Force::LinearBushing`, + * which was developed by Michael Sherman. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void setNull(); void constructProperties(); diff --git a/OpenSim/Examples/Plugins/BodyDragExample/CMakeLists.txt b/OpenSim/Examples/Plugins/BodyDragExample/CMakeLists.txt index db35b712c9..f6a33c4685 100644 --- a/OpenSim/Examples/Plugins/BodyDragExample/CMakeLists.txt +++ b/OpenSim/Examples/Plugins/BodyDragExample/CMakeLists.txt @@ -7,8 +7,8 @@ file(GLOB INCLUDE_FILES *.h) set(PLUGIN_NAME "BodyDragForce" CACHE STRING "Name of shared library to create") -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(OpenSim REQUIRED PATHS "${OPENSIM_INSTALL_DIR}") diff --git a/OpenSim/Examples/Plugins/CoupledBushingForceExample/CMakeLists.txt b/OpenSim/Examples/Plugins/CoupledBushingForceExample/CMakeLists.txt index d6d522e9f0..8cc2e9256f 100644 --- a/OpenSim/Examples/Plugins/CoupledBushingForceExample/CMakeLists.txt +++ b/OpenSim/Examples/Plugins/CoupledBushingForceExample/CMakeLists.txt @@ -8,8 +8,8 @@ file(GLOB INCLUDE_FILES *.h) set(PLUGIN_NAME "osimCoupledBushingForcePlugin" CACHE STRING "Name of shared library to create") -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(OpenSim REQUIRED PATHS "${OPENSIM_INSTALL_DIR}") diff --git a/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.cpp b/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.cpp index 9b60eedf9f..0294e14e61 100644 --- a/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.cpp +++ b/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.cpp @@ -50,7 +50,7 @@ CoupledBushingForce::~CoupledBushingForce() /** * Default constructor. */ -CoupledBushingForce::CoupledBushingForce() : TwoFrameLinker() +CoupledBushingForce::CoupledBushingForce() : TwoFrameLinker() { setAuthors("Ajay Seth"); constructProperties(); @@ -62,7 +62,7 @@ CoupledBushingForce::CoupledBushingForce( const std::string& name, const std::string& frame2Name, SimTK::Mat66 stiffnessMat, SimTK::Mat66 dampingMat) - : TwoFrameLinker(name, frame1Name, frame2Name) + : TwoFrameLinker(name, frame1Name, frame2Name) { setAuthors("Ajay Seth"); constructProperties(); @@ -104,13 +104,9 @@ void CoupledBushingForce::extendFinalizeFromProperties() //============================================================================= // COMPUTATION //============================================================================= -/* Compute the force contribution to the system and add in to appropriate - * bodyForce and/or system generalizedForce. - * CoupledBushingForce implementation based SimTK::Force::LinearBushing - * developed and implemented by Michael Sherman. */ -void CoupledBushingForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void CoupledBushingForce::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { // Calculate stiffness generalized forces of bushing by first computing // the deviation of the two frames measured by dq @@ -121,11 +117,11 @@ void CoupledBushingForce::computeForce(const SimTK::State& s, Vec6 fv = -_dampingMatrix * computeDeflectionRate(s); // total bushing force in the internal basis of the deflection (dq) - Vec6 f = fk + fv; + Vec6 f = fk + fv; - // convert internal forces to spatial and add then add to system - // physical (body) forces - addInPhysicalForcesFromInternal(s, f, bodyForces); + // convert the internal forces to into spatial forces and emit them + // into the `ForceConsumer` + producePhysicalForcesFromInternal(s, f, forceConsumer); } /** Potential energy stored in the bushing is purely a function of the deflection diff --git a/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.h b/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.h index 1585466881..0686f97c1a 100644 --- a/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.h +++ b/OpenSim/Examples/Plugins/CoupledBushingForceExample/CoupledBushingForce.h @@ -26,7 +26,8 @@ // INCLUDE #include "osimPluginDLL.h" -#include + +#include #include #include @@ -51,7 +52,7 @@ namespace OpenSim { * @author Ajay Seth */ -class OSIMPLUGIN_API CoupledBushingForce : public TwoFrameLinker { +class OSIMPLUGIN_API CoupledBushingForce : public TwoFrameLinker { OpenSim_DECLARE_CONCRETE_OBJECT(CoupledBushingForce, TwoFrameLinker); public: //============================================================================== @@ -115,21 +116,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CoupledBushingForce, TwoFrameLinker); // Uses default (compiler-generated) destructor, copy constructor, and copy // assignment operator. - //-------------------------------------------------------------------------- - // COMPUTATION - //-------------------------------------------------------------------------- - /** Compute the bushing force contribution to the system and add in to appropriate - bodyForce and/or system generalizedForce. The bushing force is [K]*dq + [D]*dqdot - where, [K] is the spatial 6dof stiffness matrix between the two frames - dq is the deflection in body spatial coordinates with rotations in Euler angles - [D] is the spatial 6dof damping matrix opposing the velocity between the frames - dqdot is the relative spatial velocity of the two frames - CoupledBushingForce implementation based SimTK::Force::LinearBushing - developed and implemented by Michael Sherman. */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - /** Potential energy is determined by the elastic energy storage of the bushing. In spatial terms, U = ~dq*[K]*dq, with K and dq defined above. */ double computePotentialEnergy(const SimTK::State& s) const override; @@ -146,9 +132,22 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CoupledBushingForce, TwoFrameLinker); */ OpenSim::Array getRecordValues(const SimTK::State& state) const override; - - private: + /** + * Implements the `ForceProducer` API by computing the bushing force's. + * + * The bushing force is [K]*dq + [D]*dqdot where: + * + * - [K] is the spatial 6dof stiffness matrix between the two frames + * - dq is the deflection in body * spatial coordinates with rotations in Euler angles + * - [D] is the spatial 6dof damping matrix opposing the velocity between the frames + * - dqdot is the relative spatial velocity of the two frames CoupledBushingForce + * + * This implementation is based on `SimTK::Force::LinearBushing`, developed + * and implemented by Michael Sherman. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + //-------------------------------------------------------------------------- // Implement ModelComponent interface. //-------------------------------------------------------------------------- diff --git a/OpenSim/Examples/SimpleOptimizationExample/CMakeLists.txt b/OpenSim/Examples/SimpleOptimizationExample/CMakeLists.txt index d5705a88e2..4cc287f198 100644 --- a/OpenSim/Examples/SimpleOptimizationExample/CMakeLists.txt +++ b/OpenSim/Examples/SimpleOptimizationExample/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET simpleOptimizationExample CACHE TYPE STRING) -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/SymbolicExpressionReporter/CMakeLists.txt b/OpenSim/Examples/SymbolicExpressionReporter/CMakeLists.txt index e4a86420e5..433daeb58f 100644 --- a/OpenSim/Examples/SymbolicExpressionReporter/CMakeLists.txt +++ b/OpenSim/Examples/SymbolicExpressionReporter/CMakeLists.txt @@ -40,8 +40,8 @@ set(PLUGIN_NAME "osimExpressionReporter") # Settings. # --------- -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Examples/checkEnvironment/CMakeLists.txt b/OpenSim/Examples/checkEnvironment/CMakeLists.txt index 5f5a7b0b4c..95e1639c57 100644 --- a/OpenSim/Examples/checkEnvironment/CMakeLists.txt +++ b/OpenSim/Examples/checkEnvironment/CMakeLists.txt @@ -6,8 +6,8 @@ cmake_minimum_required(VERSION 3.2) # --------- set(TARGET checkEnvironment CACHE STRING "Name of example to build") -# OpenSim uses C++11 language features. -set(CMAKE_CXX_STANDARD 11) +# OpenSim uses c++14 language features. +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Find and hook up to OpenSim. diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index c3b904ac21..f618f15df8 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -40,6 +40,8 @@ set(MOCO_SOURCES MocoGoal/MocoControlGoal.cpp MocoGoal/MocoControlTrackingGoal.h MocoGoal/MocoControlTrackingGoal.cpp + MocoGoal/MocoExpressionBasedParameterGoal.h + MocoGoal/MocoExpressionBasedParameterGoal.cpp MocoGoal/MocoJointReactionGoal.h MocoGoal/MocoJointReactionGoal.cpp MocoGoal/MocoOrientationTrackingGoal.h diff --git a/OpenSim/Moco/Components/StationPlaneContactForce.h b/OpenSim/Moco/Components/StationPlaneContactForce.h index d1a16642d1..f80f2618d4 100644 --- a/OpenSim/Moco/Components/StationPlaneContactForce.h +++ b/OpenSim/Moco/Components/StationPlaneContactForce.h @@ -19,6 +19,8 @@ * -------------------------------------------------------------------------- */ #include +#include +#include #include namespace OpenSim { @@ -26,8 +28,8 @@ namespace OpenSim { /** This class models compliant point contact with a ground plane y=0. @underdevelopment */ -class OSIMMOCO_API StationPlaneContactForce : public Force { -OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, Force); +class OSIMMOCO_API StationPlaneContactForce : public ForceProducer { +OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer); public: OpenSim_DECLARE_OUTPUT(force_on_station, SimTK::Vec3, calcContactForceOnStation, SimTK::Stage::Velocity); @@ -35,17 +37,6 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, Force); OpenSim_DECLARE_SOCKET(station, Station, "The body-fixed point that can contact the plane."); - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& /*generalizedForces*/) const override { - const SimTK::Vec3 force = calcContactForceOnStation(s); - const auto& pt = getConnectee("station"); - const auto& pos = pt.getLocationInGround(s); - const auto& frame = pt.getParentFrame(); - applyForceToPoint(s, frame, pt.get_location(), force, bodyForces); - applyForceToPoint(s, getModel().getGround(), pos, -force, bodyForces); - } - OpenSim::Array getRecordLabels() const override { OpenSim::Array labels; const auto stationName = getConnectee("station").getName(); @@ -71,6 +62,20 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, Force); // TODO rename to computeContactForceOnStation virtual SimTK::Vec3 calcContactForceOnStation( const SimTK::State& s) const = 0; + +private: + void implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const override { + + const SimTK::Vec3 force = calcContactForceOnStation(s); + const auto& pt = getConnectee("station"); + const auto& pos = pt.getLocationInGround(s); + const auto& frame = pt.getParentFrame(); + + forceConsumer.consumePointForce(s, frame, pt.get_location(), force); + forceConsumer.consumePointForce(s, getModel().getGround(), pos, -force); + } }; /** This class is still under development. */ diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 53dec48350..d0e3a5bb45 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -73,7 +73,7 @@ casadi::Sparsity calcJacobianSparsityWithPerturbation(const VectorDM& x0s, return combinedSparsity; } -casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, +casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, bool symmetric) const { using casadi::DM; using casadi::Slice; @@ -193,7 +193,7 @@ casadi::Sparsity Endpoint::get_sparsity_in(casadi_int i) { return casadi::Sparsity(0, 0); } } -casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, +casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { using casadi::Slice; if (i == 0) { @@ -243,8 +243,8 @@ VectorDM EndpointConstraint::eval(const VectorDM& args) const { return out; } -template -casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( +template +casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( @@ -256,9 +256,9 @@ casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - if (CalcKCErrors) { - int numRows = m_casProblem->getNumKinematicConstraintEquations(); - return casadi::Sparsity::dense(numRows, 1); + if (calcKCErr) { + return casadi::Sparsity::dense( + m_casProblem->getNumKinematicConstraintEquations(), 1); } else { return casadi::Sparsity(0, 0); } @@ -267,9 +267,8 @@ casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( } } -template -VectorDM MultibodySystemExplicit::eval( - const VectorDM& args) const { +template +VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -278,7 +277,7 @@ VectorDM MultibodySystemExplicit::eval( } Problem::MultibodySystemExplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemExplicit(input, CalcKCErrors, output); + m_casProblem->calcMultibodySystemExplicit(input, calcKCErr, output); return out; } @@ -290,9 +289,7 @@ casadi::Sparsity VelocityCorrection::get_sparsity_in(casadi_int i) { return casadi::Sparsity::dense(1, 1); } else if (i == 1) { return casadi::Sparsity::dense( - m_casProblem->getNumStates() - - m_casProblem->getNumAuxiliaryStates(), - 1); + m_casProblem->getNumMultibodyStates(), 1); } else if (i == 2) { return casadi::Sparsity::dense(m_casProblem->getNumSlacks(), 1); } else if (i == 3) { @@ -314,8 +311,7 @@ casadi::DM VelocityCorrection::getSubsetPoint( const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; using casadi::Slice; - const int NMBS = m_casProblem->getNumStates() - - m_casProblem->getNumAuxiliaryStates(); + const int NMBS = m_casProblem->getNumMultibodyStates(); if (i == 0) { return fullPoint.at(initial_time); } else if (i == 1) { @@ -336,8 +332,57 @@ VectorDM VelocityCorrection::eval(const VectorDM& args) const { return out; } -template -casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( +casadi::Sparsity StateProjection::get_sparsity_in(casadi_int i) { + if (i == 0) { + return casadi::Sparsity::dense(1, 1); + } else if (i == 1) { + return casadi::Sparsity::dense( + m_casProblem->getNumMultibodyStates(), 1); + } else if (i == 2) { + return casadi::Sparsity::dense(m_casProblem->getNumSlacks(), 1); + } else if (i == 3) { + return casadi::Sparsity::dense(m_casProblem->getNumParameters(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} + +casadi::Sparsity StateProjection::get_sparsity_out(casadi_int i) { + if (i == 0) { + return casadi::Sparsity::dense( + m_casProblem->getNumMultibodyStates(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} + +casadi::DM StateProjection::getSubsetPoint( + const VariablesDM& fullPoint, casadi_int i) const { + int itime = 0; + using casadi::Slice; + const int NMBS = m_casProblem->getNumMultibodyStates(); + if (i == 0) { + return fullPoint.at(initial_time); + } else if (i == 1) { + return fullPoint.at(states)(Slice(0, NMBS), itime); + } else if (i == 2) { + return fullPoint.at(slacks)(Slice(), itime); + } else if (i == 3) { + return fullPoint.at(parameters); + } else { + return casadi::DM(); + } +} + +VectorDM StateProjection::eval(const VectorDM& args) const { + VectorDM out{casadi::DM(sparsity_out(0))}; + m_casProblem->calcStateProjection( + args.at(0).scalar(), args.at(1), args.at(2), args.at(3), out[0]); + return out; +} + +template +casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( @@ -349,9 +394,9 @@ casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - if (CalcKCErrors) { - int numRows = m_casProblem->getNumKinematicConstraintEquations(); - return casadi::Sparsity::dense(numRows, 1); + if (calcKCErr) { + return casadi::Sparsity::dense( + m_casProblem->getNumKinematicConstraintEquations(), 1); } else { return casadi::Sparsity(0, 0); } @@ -360,9 +405,8 @@ casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( } } -template -VectorDM MultibodySystemImplicit::eval( - const VectorDM& args) const { +template +VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -370,9 +414,9 @@ VectorDM MultibodySystemImplicit::eval( out[i] = casadi::DM(sparsity_out(i)); } - Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], + Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemImplicit(input, CalcKCErrors, output); + m_casProblem->calcMultibodySystemImplicit(input, calcKCErr, output); return out; } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index f816f34b46..8a1ccef08c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -61,7 +61,7 @@ class Function : public casadi::Callback { bool has_jac_sparsity(casadi_int oind, casadi_int iind) const override { return !m_fullPointsForSparsityDetection->empty(); } - casadi::Sparsity get_jac_sparsity(casadi_int oind, casadi_int iind, + casadi::Sparsity get_jac_sparsity(casadi_int oind, casadi_int iind, bool symmetric) const override; protected: @@ -69,7 +69,7 @@ class Function : public casadi::Callback { private: /// Here, "point" refers to a vector of all variables in the optimization - /// problem. This function returns a subset of the variables at a point for + /// problem. This function returns a subset of the variables at a point for /// a given input index. VectorDM getSubsetPointsForSparsityDetection(casadi_int iind) const { VectorDM out(m_fullPointsForSparsityDetection->size()); @@ -81,9 +81,9 @@ class Function : public casadi::Callback { } /// As of CasADi 3.6, callback functions need to return Jacobian sparsity - /// patterns for each pair of inputs and outputs. Therefore this function + /// patterns for each pair of inputs and outputs. Therefore this function /// returns a subset point for a given input index. - virtual casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + virtual casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; using casadi::Slice; @@ -233,7 +233,7 @@ class Endpoint : public Function { /// provided point, but applying the integrand function and quadrature /// scheme here is complicated. For simplicity, we provide the integral as /// 0. - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const override; protected: @@ -256,7 +256,7 @@ class EndpointConstraint : public Endpoint { /// This function should compute forward dynamics (explicit multibody dynamics), /// auxiliary explicit dynamics, and the errors for the kinematic constraints. -template +template class MultibodySystemExplicit : public Function { public: casadi_int get_n_out() override final { return 4; } @@ -297,11 +297,39 @@ class VelocityCorrection : public Function { casadi::Sparsity get_sparsity_in(casadi_int i) override final; casadi::Sparsity get_sparsity_out(casadi_int i) override final; VectorDM eval(const VectorDM& args) const override; - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const override; }; -template +/// This function should compute a state projection term to make feasible +/// problems that enforce kinematic constraints and their derivatives. +class StateProjection : public Function { +public: + casadi_int get_n_in() override final { return 4; } + casadi_int get_n_out() override final { return 1; } + std::string get_name_in(casadi_int i) override final { + switch (i) { + case 0: return "time"; + case 1: return "multibody_states"; + case 2: return "slacks"; + case 3: return "parameters"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + std::string get_name_out(casadi_int i) override final { + switch (i) { + case 0: return "state_projection"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + casadi::Sparsity get_sparsity_in(casadi_int i) override final; + casadi::Sparsity get_sparsity_out(casadi_int i) override final; + VectorDM eval(const VectorDM& args) const override; + casadi::DM getSubsetPoint( + const VariablesDM& fullPoint, casadi_int i) const override; +}; + +template class MultibodySystemImplicit : public Function { casadi_int get_n_out() override final { return 4; } std::string get_name_out(casadi_int i) override final { @@ -313,6 +341,7 @@ class MultibodySystemImplicit : public Function { default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); } } + casadi::Sparsity get_sparsity_out(casadi_int i) override final; VectorDM eval(const VectorDM& args) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index b5dd2ff97a..f815638e3c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -50,30 +50,21 @@ DM HermiteSimpson::createMeshIndicesImpl() const { return indices; } -void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); - - int time_i; - int time_mid; - int time_ip1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - time_i = 2 * imesh; // Needed for defects and path constraints. - // We enforce defect constraints on a mesh interval basis, so add // constraints until the number of mesh intervals is reached. - time_mid = 2 * imesh + 1; - time_ip1 = 2 * imesh + 2; - - const auto h = m_times(time_ip1) - m_times(time_i); - const auto x_i = x(Slice(), time_i); - const auto x_mid = x(Slice(), time_mid); - const auto x_ip1 = x(Slice(), time_ip1); - const auto xdot_i = xdot(Slice(), time_i); - const auto xdot_mid = xdot(Slice(), time_mid); - const auto xdot_ip1 = xdot(Slice(), time_ip1); + const auto h = m_times(2 * imesh + 2) - m_times(2 * imesh); + const auto x_i = x[imesh](Slice(), 0); + const auto x_mid = x[imesh](Slice(), 1); + const auto x_ip1 = x[imesh](Slice(), 2); + const auto xdot_i = xdot[imesh](Slice(), 0); + const auto xdot_mid = xdot[imesh](Slice(), 1); + const auto xdot_ip1 = xdot[imesh](Slice(), 2); // Hermite interpolant defects. defects(Slice(0, NS), imesh) = @@ -88,7 +79,7 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, void HermiteSimpson::calcInterpolatingControlsImpl( const casadi::MX& controls, casadi::MX& interpControls) const { if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { + m_solver.getInterpolateControlMeshInteriorPoints()) { int time_i; int time_mid; int time_ip1; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 8c7f25f428..ff11784445 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -37,9 +37,12 @@ namespace CasOC { /// /// Kinematic constraints and path constraints. /// ------------------------------------------- -/// Kinematic constraint and path constraint errors are enforced only at the -/// mesh points. Errors at collocation points at the mesh interval midpoint -/// are ignored. +/// Position- and velocity-level kinematic constraint errors and path constraint +/// errors are enforced only at the mesh points. In the kinematic constraint +/// method by Bordalba et al. (2023), the acceleration-level constraints are +/// also enforced at the collocation points. In the kinematic constraint method +/// by Posa et al. (2016), the acceleration-level constraints are only enforced +/// at the mesh points. class HermiteSimpson : public Transcription { public: HermiteSimpson(const Solver& solver, const Problem& problem) @@ -47,7 +50,8 @@ class HermiteSimpson : public Transcription { casadi::DM grid = casadi::DM::zeros(1, (2 * m_solver.getMesh().size()) - 1); const auto& mesh = m_solver.getMesh(); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); + const bool interpControls = + m_solver.getInterpolateControlMeshInteriorPoints(); casadi::DM pointsForInterpControls; if (interpControls) { pointsForInterpControls = @@ -63,15 +67,16 @@ class HermiteSimpson : public Transcription { } } } - createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), + createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, + casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 365c604a75..f65eff500b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -39,6 +39,9 @@ enum Var { derivatives, // TODO: Rename to accelerations? /// Constant in time. parameters, + /// A "mirror" of the multibody states used in the projection method + /// for solving kinematic constraints. + projection_states, /// For internal use (never actually a key for Variables). multibody_states = 100 }; @@ -61,10 +64,12 @@ struct Iterate { std::vector slack_names; std::vector derivative_names; std::vector parameter_names; + std::vector projection_state_names; int iteration = -1; /// Return a new iterate in which the data is resampled at the times in /// newTimes. - Iterate resample(const casadi::DM& newTimes) const; + Iterate resample(const casadi::DM& newTimes, + bool appendProjectionStates) const; }; /// This struct is used to return a solution to a problem. Use `stats` diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 33db331af3..50e8a81756 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -54,18 +54,17 @@ DM LegendreGauss::createMeshIndicesImpl() const { return indices; } -void LegendreGauss::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGauss::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); const auto h = m_times(igrid + m_degree + 1) - m_times(igrid); - const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); - const auto xdot_i = xdot(Slice(), - Slice(igrid + 1, igrid + m_degree + 1)); - const auto x_ip1 = x(Slice(), igrid + m_degree + 1); + const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1)); + const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1)); + const auto x_ip1 = x[imesh](Slice(), m_degree + 1); // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); @@ -82,7 +81,7 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, void LegendreGauss::calcInterpolatingControlsImpl( const casadi::MX& controls, casadi::MX& interpControls) const { if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { + m_solver.getInterpolateControlMeshInteriorPoints()) { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); const auto c_i = controls(Slice(), igrid); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 493801633f..02ac20834c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -47,9 +47,10 @@ namespace CasOC { /// /// Kinematic constraints and path constraints. /// ------------------------------------------- -/// Kinematic constraint and path constraint errors are enforced only at the -/// mesh points. Errors at collocation points within the mesh interval midpoint -/// are ignored. +/// Position- and velocity-level kinematic constraint errors and path constraint +/// errors are enforced only at the mesh points. In the kinematic constraint +/// method by Bordalba et al. [3], the acceleration-level constraints are also +/// enforced at the collocation points. /// /// References /// ---------- @@ -71,7 +72,8 @@ class LegendreGauss : public Transcription { const int numMeshIntervals = (int)mesh.size() - 1; const int numGridPoints = (int)mesh.size() + numMeshIntervals * m_degree; casadi::DM grid = casadi::DM::zeros(1, numGridPoints); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); + const bool interpControls = + m_solver.getInterpolateControlMeshInteriorPoints(); casadi::DM pointsForInterpControls; if (interpControls) { pointsForInterpControls = casadi::DM::zeros(1, @@ -105,14 +107,15 @@ class LegendreGauss : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, (m_degree + 1) * m_problem.getNumStates(), + m_degree + 2, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 281a753987..915ff8ba82 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -52,17 +52,16 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { return indices; } -void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGaussRadau::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; const auto h = m_times(igrid + m_degree) - m_times(igrid); - const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); - const auto xdot_i = xdot(Slice(), - Slice(igrid + 1, igrid + m_degree + 1)); + const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1)); + const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1)); // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); @@ -75,7 +74,7 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, void LegendreGaussRadau::calcInterpolatingControlsImpl( const casadi::MX& controls, casadi::MX& interpControls) const { if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { + m_solver.getInterpolateControlMeshInteriorPoints()) { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; const auto c_i = controls(Slice(), igrid); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 191cb3990f..ebd4fef4f7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -46,9 +46,10 @@ namespace CasOC { /// /// Kinematic constraints and path constraints. /// ------------------------------------------- -/// Kinematic constraint and path constraint errors are enforced only at the -/// mesh points. Errors at collocation points within the mesh interval midpoint -/// are ignored. +/// Position- and velocity-level kinematic constraint errors and path constraint +/// errors are enforced only at the mesh points. In the kinematic constraint +/// method by Bordalba et al. [3], the acceleration-level constraints are also +/// enforced at the collocation points. /// /// References /// ---------- @@ -71,7 +72,8 @@ class LegendreGaussRadau : public Transcription { const int numGridPoints = (int)mesh.size() + numMeshIntervals * (m_degree - 1); casadi::DM grid = casadi::DM::zeros(1, numGridPoints); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); + const bool interpControls = + m_solver.getInterpolateControlMeshInteriorPoints(); casadi::DM pointsForInterpControls; if (interpControls) { pointsForInterpControls = casadi::DM::zeros(1, @@ -107,14 +109,15 @@ class LegendreGaussRadau : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), + m_degree + 1, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp index 740cf59782..172aa879eb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp @@ -31,14 +31,16 @@ using OpenSim::Exception; namespace CasOC { -Iterate Iterate::resample(const casadi::DM& newTimes) const { +Iterate Iterate::resample(const casadi::DM& newTimes, + bool appendProjectionStates = false) const { // Since we are converting to a MocoTrajectory and immediately converting // back to a CasOC::Iterate after resampling, we do not need to provide // Input control indexes, even if they are present in the MocoProblem. auto mocoTraj = OpenSim::convertToMocoTrajectory(*this); auto simtkNewTimes = OpenSim::convertToSimTKVector(newTimes); mocoTraj.resample(simtkNewTimes); - return OpenSim::convertToCasOCIterate(mocoTraj); + return OpenSim::convertToCasOCIterate(mocoTraj, mocoTraj.getSlackNames(), + appendProjectionStates); } std::vector diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h index 6d729123a6..94fc3049fd 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h @@ -22,7 +22,6 @@ #include "CasOCFunction.h" #include #include -#include namespace OpenSim { class MocoCasADiSolver; @@ -208,15 +207,17 @@ class Problem { std::move(multInitialBounds), std::move(multFinalBounds), kinLevel}); - if (kinLevel == KinematicLevel::Position) - ++m_numHolonomicConstraintEquations; - else if (kinLevel == KinematicLevel::Velocity) - ++m_numNonHolonomicConstraintEquations; - else if (kinLevel == KinematicLevel::Acceleration) - ++m_numAccelerationConstraintEquations; - } - /// Add a slack velocity correction variable to the problem associated with - /// a kinematic constraint in the model. + if (!isPrescribedKinematics()) { + if (kinLevel == KinematicLevel::Position) + ++m_numHolonomicConstraintEquations; + else if (kinLevel == KinematicLevel::Velocity) + ++m_numNonHolonomicConstraintEquations; + else if (kinLevel == KinematicLevel::Acceleration) + ++m_numAccelerationConstraintEquations; + } + } + /// Add a slack variable to the problem associated with a kinematic + /// constraint in the model. void addSlack(std::string name, Bounds bounds) { m_slackInfos.push_back({std::move(name), std::move(bounds)}); } @@ -240,6 +241,15 @@ class Problem { void setKinematicConstraintBounds(Bounds bounds) { m_kinematicConstraintBounds = std::move(bounds); } + /// Set the method used to enforce kinematic constraints. + void setKinematicConstraintMethod(std::string method) { + OPENSIM_THROW_IF(method != "Bordalba2023" && method != "Posa2016", + OpenSim::Exception, + "Invalid method kinematic constraint method.") + m_kinematicConstraintMethod = std::move(method); + m_isKinematicConstraintMethodBordalba2023 = + m_kinematicConstraintMethod == "Bordalba2023"; + } /// Add a constant (time-invariant) variable to the optimization problem. void addParameter(std::string name, Bounds bounds) { m_paramInfos.push_back({std::move(name), std::move(bounds)}); @@ -316,7 +326,10 @@ class Problem { const casadi::DM& multibody_states, const casadi::DM& slacks, const casadi::DM& parameters, casadi::DM& velocity_correction) const = 0; - + virtual void calcStateProjection(const double& time, + const casadi::DM& multibody_states, const casadi::DM& slacks, + const casadi::DM& parameters, + casadi::DM& projection) const = 0; virtual void calcCostIntegrand(int /*costIndex*/, const ContinuousInput& /*input*/, double& /*integrand*/) const {} virtual void calcCost(int /*costIndex*/, const CostInput& /*input*/, @@ -370,10 +383,31 @@ class Problem { } } } + if (getNumKinematicConstraintEquations() && + isKinematicConstraintMethodBordalba2023()) { + for (const auto& info : m_stateInfos) { + if (info.type == StateType::Coordinate) { + auto name = info.name; + auto leafpos = name.find("/value"); + OPENSIM_THROW_IF(leafpos == std::string::npos, + OpenSim::Exception, "Internal error."); + name.replace(leafpos, 6, "/value/projection"); + it.projection_state_names.push_back(name); + } + if (info.type == StateType::Speed) { + auto name = info.name; + auto leafpos = name.find("/speed"); + OPENSIM_THROW_IF(leafpos == std::string::npos, + OpenSim::Exception, "Internal error."); + name.replace(leafpos, 6, "/speed/projection"); + it.projection_state_names.push_back(name); + } + } + } for (const auto& auxDerivName : m_auxiliaryDerivativeNames) { it.derivative_names.push_back(auxDerivName); } - + for (const auto& info : m_paramInfos) it.parameter_names.push_back(info.name); return it; @@ -456,12 +490,21 @@ class Problem { pointsForSparsityDetection); } - if (m_enforceConstraintDerivatives) { - mutThis->m_velocityCorrectionFunc = + if (getEnforceConstraintDerivatives() && + getNumKinematicConstraintEquations()) { + if (isKinematicConstraintMethodBordalba2023()) { + mutThis->m_stateProjectionFunc = + OpenSim::make_unique(); + mutThis->m_stateProjectionFunc->constructFunction(this, + "state_projection", finiteDiffScheme, + pointsForSparsityDetection); + } else { + mutThis->m_velocityCorrectionFunc = OpenSim::make_unique(); - mutThis->m_velocityCorrectionFunc->constructFunction(this, + mutThis->m_velocityCorrectionFunc->constructFunction(this, "velocity_correction", finiteDiffScheme, pointsForSparsityDetection); + } } } @@ -476,6 +519,9 @@ class Problem { int getNumDerivatives() const { return getNumAccelerations() + getNumAuxiliaryResidualEquations(); } + int getNumProjectionStates() const { + return getNumProjectionConstraintEquations(); + } int getNumSlacks() const { return (int)m_slackInfos.size(); } /// This is the number of generalized coordinates, which may be greater /// than the number of generalized speeds. @@ -488,6 +534,7 @@ class Problem { return 0; } } + int getNumMultibodyStates() const { return m_numCoordinates + m_numSpeeds; } int getNumAuxiliaryStates() const { return m_numAuxiliaryStates; } int getNumCosts() const { return (int)m_costInfos.size(); } bool isPrescribedKinematics() const { return m_prescribedKinematics; } @@ -505,19 +552,32 @@ class Problem { int getNumAuxiliaryResidualEquations() const { return m_numAuxiliaryResiduals; } - int getNumKinematicConstraintEquations() const { + int getNumQErr() const { // If all kinematics are prescribed, we assume that the prescribed // kinematics obey any kinematic constraints. Therefore, the kinematic // constraints would be redundant, and we need not enforce them. + if (m_prescribedKinematics) return 0; + return m_numHolonomicConstraintEquations; + } + int getNumUErr() const { if (m_prescribedKinematics) return 0; if (m_enforceConstraintDerivatives) { - return 3 * m_numHolonomicConstraintEquations + - 2 * m_numNonHolonomicConstraintEquations + + return m_numHolonomicConstraintEquations + + m_numNonHolonomicConstraintEquations; + } + return m_numNonHolonomicConstraintEquations; + } + int getNumUDotErr() const { + if (m_prescribedKinematics) return 0; + if (m_enforceConstraintDerivatives) { + return m_numHolonomicConstraintEquations + + m_numNonHolonomicConstraintEquations + m_numAccelerationConstraintEquations; } - return m_numHolonomicConstraintEquations + - m_numNonHolonomicConstraintEquations + - m_numAccelerationConstraintEquations; + return m_numAccelerationConstraintEquations; + } + int getNumKinematicConstraintEquations() const { + return getNumQErr() + getNumUErr() + getNumUDotErr(); } /// Create a vector of names for scalar kinematic constraint equations. /// The length of the vector is getNumKinematicConstraintEquations(). @@ -546,6 +606,21 @@ class Problem { const Bounds& getKinematicConstraintBounds() const { return m_kinematicConstraintBounds; } + std::string getKinematicConstraintMethod() const { + return m_kinematicConstraintMethod; + } + bool isKinematicConstraintMethodBordalba2023() const { + return m_isKinematicConstraintMethodBordalba2023; + } + int getNumProjectionConstraintEquations() const { + if ((getNumQErr() + getNumUErr()) && + isKinematicConstraintMethodBordalba2023()) { + return getNumMultibodyStates(); + } else { + return 0; + } + + } const Bounds& getTimeInitialBounds() const { return m_timeInitialBounds; } const Bounds& getTimeFinalBounds() const { return m_timeFinalBounds; } const std::vector& getStateInfos() const { return m_stateInfos; } @@ -580,12 +655,18 @@ class Problem { return *m_multibodyFuncIgnoringConstraints; } /// Get a function to compute the velocity correction to qdot when enforcing - /// kinematic constraints and their derivatives. We require a separate - /// function for this since we don't actually compute qdot within the - /// multibody system. + /// kinematic constraints and their derivatives using the method by + /// Posa et al. (2016). We require a separate function for this since we don't + /// actually compute qdot within the multibody system. const casadi::Function& getVelocityCorrection() const { return *m_velocityCorrectionFunc; } + /// Get a function to compute the state projection term when enforcing + /// kinematic constraints and their derivatives using the method by + /// Bordalba et al. (2023). + const casadi::Function& getStateProjection() const { + return *m_stateProjectionFunc; + } const casadi::Function& getImplicitMultibodySystem() const { return *m_implicitMultibodyFunc; } @@ -614,8 +695,10 @@ class Problem { int m_numAccelerationConstraintEquations = 0; bool m_enforceConstraintDerivatives = false; std::string m_dynamicsMode = "explicit"; + std::string m_kinematicConstraintMethod = "Posa2016"; std::vector m_auxiliaryDerivativeNames; bool m_isDynamicsModeImplicit = false; + bool m_isKinematicConstraintMethodBordalba2023 = false; bool m_prescribedKinematics = false; int m_numMultibodyDynamicsEquationsIfPrescribedKinematics = 0; Bounds m_kinematicConstraintBounds; @@ -633,6 +716,7 @@ class Problem { std::unique_ptr> m_implicitMultibodyFuncIgnoringConstraints; std::unique_ptr m_velocityCorrectionFunc; + std::unique_ptr m_stateProjectionFunc; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index 7bc3f4a456..4ed090fbff 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -125,7 +125,10 @@ Solution Solver::solve(const Iterate& guess) const { const auto guessTimes = transcription->createTimes(guessCopy.variables.at(initial_time), guessCopy.variables.at(final_time)); - guessCopy = guessCopy.resample(guessTimes); + bool appendProjectionStates = + m_problem.getNumKinematicConstraintEquations() && + m_problem.isKinematicConstraintMethodBordalba2023(); + guessCopy = guessCopy.resample(guessTimes, appendProjectionStates); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { // Make sure the exact same sparsity pattern is used every time. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h index ec0951dc04..37464a0505 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h @@ -104,26 +104,48 @@ class Solver { void setImplicitAuxiliaryDerivativesWeight(double weight) { m_implicitAuxiliaryDerivativesWeight = weight; } - - /// Whether or not to constrain control values at mesh interval midpoints - /// by linearly interpolating control values from mesh interval endpoints. - /// @note Only applies to Hermite-Simpson collocation. - void setInterpolateControlMidpoints(bool tf) { - m_interpolateControlMidpoints = tf; + bool getMinimizeStateProjection() const { + return m_minimizeStateProjection; + } + void setMinimizeStateProjection(bool tf) { + m_minimizeStateProjection = tf; } - bool getInterpolateControlMidpoints() const { - return m_interpolateControlMidpoints; + double getStateProjectionWeight() const { + return m_stateProjectionWeight; + } + void setStateProjectionWeight(double weight) { + m_stateProjectionWeight = weight; } - /// Whether or not to enforce path constraints at mesh interval midpoints. - /// @note Only applies to Hermite-Simpson collocation. + /// Whether or not to constrain control values interior to the mesh interval + /// by linearly interpolating control values from mesh interval endpoints. + /// + /// @note For Hermite-Simpson collocation, this applies to the time point + /// at the midpoint of the mesh interval. For Legendre-Gauss and + /// Legendre-Gauss-Radau collocation, this applies to all time points + /// in the interior of the mesh interval. + void setInterpolateControlMeshInteriorPoints(bool tf) { + m_interpolateControlMeshInteriorPoints = tf; + } + /// @copydoc setInterpolateControlMidpoints() + bool getInterpolateControlMeshInteriorPoints() const { + return m_interpolateControlMeshInteriorPoints; + } + + /// Whether or not to enforce path constraints at points interior to the + /// mesh interval. + /// + /// @note For Hermite-Simpson collocation, this applies to the time point + /// at the midpoint of the mesh interval. For Legendre-Gauss and + /// Legendre-Gauss-Radau collocation, this applies to all time points + /// in the interior of the mesh interval. /// @note Does not apply to implicit dynamics residuals, as these are /// always enforced at mesh interval midpoints. - void setEnforcePathConstraintMidpoints(bool tf) { - m_enforcePathConstraintMidpoints = tf; + void setEnforcePathConstraintMeshInteriorPoints(bool tf) { + m_enforcePathConstraintMeshInteriorPoints = tf; } - bool getEnforcePathConstraintMidpoints() const { - return m_enforcePathConstraintMidpoints; + bool getEnforcePathConstraintMeshInteriorPoints() const { + return m_enforcePathConstraintMeshInteriorPoints; } void setOptimSolver(std::string optimSolver) { @@ -201,8 +223,10 @@ class Solver { double m_implicitMultibodyAccelerationsWeight = 1.0; bool m_minimizeImplicitAuxiliaryDerivatives = false; double m_implicitAuxiliaryDerivativesWeight = 1.0; - bool m_interpolateControlMidpoints = true; - bool m_enforcePathConstraintMidpoints = false; + bool m_minimizeStateProjection = true; + double m_stateProjectionWeight = 1e-6; + bool m_interpolateControlMeshInteriorPoints = true; + bool m_enforcePathConstraintMeshInteriorPoints = false; Bounds m_implicitMultibodyAccelerationBounds; Bounds m_implicitAuxiliaryDerivativeBounds; std::string m_finite_difference_scheme = "central"; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index a2738647a7..08adbe140b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -1,9 +1,9 @@ /* -------------------------------------------------------------------------- * * OpenSim Moco: CasOCTranscription.cpp * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -82,6 +82,7 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numPointsPerMeshInterval, const casadi::DM& pointsForInterpControls) { // Set the grid. // ------------- @@ -94,18 +95,26 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshIntervals = m_numMeshPoints - 1; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; + m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_pointsForInterpControls = pointsForInterpControls; m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() : 0; m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); + m_numProjectionStates = m_problem.getNumProjectionConstraintEquations(); + m_numUDotErrorPoints = m_problem.isKinematicConstraintMethodBordalba2023() + ? m_numGridPoints + : m_numMeshPoints; m_numConstraints = m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + - m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints + - m_problem.getNumControls() * (int)pointsForInterpControls.numel(); + m_problem.getNumQErr() * m_numMeshPoints + + m_problem.getNumUErr() * m_numMeshPoints + + m_problem.getNumUDotErr() * m_numUDotErrorPoints + + m_problem.getNumControls() * (int)pointsForInterpControls.numel() + + m_problem.getNumProjectionConstraintEquations() * m_numMeshIntervals; m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { @@ -113,8 +122,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numConstraints += info.num_outputs; } m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); - m_numPathConstraintPoints = m_solver.getEnforcePathConstraintMidpoints() - ? m_numGridPoints : m_numMeshPoints; + m_numPathConstraintPoints = + m_solver.getEnforcePathConstraintMeshInteriorPoints() + ? m_numGridPoints : m_numMeshPoints; for (int ipc = 0; ipc < (int)m_constraints.path.size(); ++ipc) { const auto& info = m_problem.getPathConstraintInfos()[ipc]; m_numConstraints += info.size() * m_numPathConstraintPoints; @@ -127,28 +137,60 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[final_time] = MX::sym("final_time"); m_scaledVars[states] = MX::sym("states", m_problem.getNumStates(), m_numGridPoints); + m_scaledVars[projection_states] = MX::sym( + "projection_states", m_numProjectionStates, m_numMeshIntervals); m_scaledVars[controls] = MX::sym("controls", m_problem.getNumControls(), m_numGridPoints); m_scaledVars[multipliers] = MX::sym( "multipliers", m_problem.getNumMultipliers(), m_numGridPoints); m_scaledVars[derivatives] = MX::sym( "derivatives", m_problem.getNumDerivatives(), m_numGridPoints); - - // TODO: This assumes that slack variables are applied at all - // collocation points on the mesh interval interior. - m_scaledVars[slacks] = MX::sym( - "slacks", m_problem.getNumSlacks(), m_numMeshInteriorPoints); m_scaledVars[parameters] = MX::sym("parameters", m_problem.getNumParameters(), 1); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + // In the projection method for enforcing kinematic constraints, the + // slack variables are applied at the mesh points at the end of each + // mesh interval. + m_scaledVars[slacks] = MX::sym( + "slacks", m_problem.getNumSlacks(), m_numMeshIntervals); + } else { + // In the Posa et al. 2016 method for enforcing kinematic constraints, + // the mesh interior points will be the mesh interval midpoints in the + // Hermite-Simpson collocation scheme. + m_scaledVars[slacks] = MX::sym( + "slacks", m_problem.getNumSlacks(), m_numMeshInteriorPoints); + } + + // Each vector contains MX matrix elements (states or state derivatives) + // needed to construct the defect constraints for an individual mesh + // interval. + m_statesByMeshInterval = MXVector(m_numMeshIntervals); + m_stateDerivativesByMeshInterval = MXVector(m_numMeshIntervals); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_statesByMeshInterval[imesh] = + MX(m_problem.getNumStates(), m_numPointsPerMeshInterval); + m_stateDerivativesByMeshInterval[imesh] = + MX(m_problem.getNumStates(), m_numPointsPerMeshInterval); + } + m_projectionStateDistances = MX(m_numProjectionStates, m_numMeshIntervals); + m_meshIndicesMap = createMeshIndices(); std::vector meshIndicesVector; std::vector meshInteriorIndicesVector; + std::vector projectionStateIndicesVector; + std::vector notProjectionStateIndicesVector; for (int i = 0; i < m_meshIndicesMap.size2(); ++i) { if (m_meshIndicesMap(i).scalar() == 1) { meshIndicesVector.push_back(i); + if (i == 0) { + notProjectionStateIndicesVector.push_back(i); + } else { + projectionStateIndicesVector.push_back(i); + } } else { meshInteriorIndicesVector.push_back(i); + notProjectionStateIndicesVector.push_back(i); } } @@ -165,9 +207,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_meshIndices = makeTimeIndices(meshIndicesVector); m_meshInteriorIndices = makeTimeIndices(meshInteriorIndicesVector); - m_pathConstraintIndices = m_solver.getEnforcePathConstraintMidpoints() - ? makeTimeIndices(gridIndicesVector) - : makeTimeIndices(meshIndicesVector); + m_pathConstraintIndices = + m_solver.getEnforcePathConstraintMeshInteriorPoints() + ? makeTimeIndices(gridIndicesVector) + : makeTimeIndices(meshIndicesVector); + m_projectionStateIndices = makeTimeIndices(projectionStateIndicesVector); + m_notProjectionStateIndices = + makeTimeIndices(notProjectionStateIndicesVector); // Set variable bounds. // -------------------- @@ -246,11 +292,11 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, if (m_problem.getNumAuxiliaryResidualEquations()) { setVariableBounds(derivatives, Slice(m_problem.getNumAccelerations(), - m_problem.getNumDerivatives()), + m_problem.getNumDerivatives()), Slice(), m_solver.getImplicitAuxiliaryDerivativeBounds()); setVariableScaling(derivatives, Slice(m_problem.getNumAccelerations(), - m_problem.getNumDerivatives()), + m_problem.getNumDerivatives()), Slice(), m_solver.getImplicitAuxiliaryDerivativeBounds()); } } @@ -263,6 +309,20 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, ++isl; } } + { + // Use the bounds from the "true" multibody states to set the bounds for + // the projection states. + const auto& stateInfos = m_problem.getStateInfos(); + for (int ips = 0; ips < m_numProjectionStates; ++ips) { + const auto& info = stateInfos[ips]; + setVariableBounds( + projection_states, ips, Slice(0, m_numMeshIntervals - 1), + info.bounds); + setVariableBounds(projection_states, ips, -1, info.finalBounds); + setVariableScaling(projection_states, Slice(), Slice(), + info.bounds); + } + } { const auto& paramInfos = m_problem.getParameterInfos(); int ip = 0; @@ -281,11 +341,49 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, MX::repmat(m_unscaledVars[parameters], 1, m_numGridPoints); m_paramsTrajMesh = MX::repmat(m_unscaledVars[parameters], 1, m_numMeshPoints); - m_paramsTrajMeshInterior = MX::repmat(m_unscaledVars[parameters], 1, - m_numMeshInteriorPoints); + m_paramsTrajMeshInterior = + MX::repmat(m_unscaledVars[parameters], 1, m_numMeshInteriorPoints); m_paramsTrajPathCon = - MX::repmat(m_unscaledVars[parameters], 1, - m_numPathConstraintPoints); + MX::repmat(m_unscaledVars[parameters], 1, m_numPathConstraintPoints); + m_paramsTrajProjState = + MX::repmat(m_unscaledVars[parameters], 1, m_numMeshIntervals); + + casadi_int istart = 0; + int numStates = m_problem.getNumStates(); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + casadi_int numPts = m_numPointsPerMeshInterval; + casadi_int iend = istart + numPts - 1; + if (m_numProjectionStates) { + // The states at all points in the mesh interval except the last + // point are the regular state variables. + m_statesByMeshInterval[imesh](Slice(), Slice(0, numPts-1)) = + m_unscaledVars[states](Slice(), Slice(istart, iend)); + + // The multibody states at the last point in the mesh interval are + // the projection states. + m_statesByMeshInterval[imesh] + (Slice(0, m_numProjectionStates), numPts-1) = + m_unscaledVars[projection_states](Slice(), imesh); + + // The non-multibody states at the last point (i.e., auxiliary state + // variables for muscles) are also the same as the regular state + // variables (there are no projection states for these variables). + m_statesByMeshInterval[imesh]( + Slice(m_numProjectionStates, numStates), numPts-1) = + m_unscaledVars[states]( + Slice(m_numProjectionStates, numStates), iend); + + // Calculate the distance between the regular multibody states and + // the projection multibody states. + m_projectionStateDistances(Slice(), imesh) = + m_unscaledVars[projection_states](Slice(), imesh) - + m_unscaledVars[states](Slice(0, m_numProjectionStates), iend); + } else { + m_statesByMeshInterval[imesh](Slice(), Slice()) = + m_unscaledVars[states](Slice(), Slice(istart, iend+1)); + } + istart = iend; + } } void Transcription::transcribe() { @@ -308,6 +406,7 @@ void Transcription::transcribe() { // Initialize memory for state derivatives and defects. // ---------------------------------------------------- m_xdot = MX(NS, m_numGridPoints); + m_xdot_projection = MX(m_numProjectionStates, m_numMeshIntervals); m_constraints.defects = MX(casadi::Sparsity::dense( m_numDefectsPerMeshInterval, m_numMeshIntervals)); m_constraintsLowerBounds.defects = @@ -335,48 +434,115 @@ void Transcription::transcribe() { // Initialize memory for kinematic constraints. // -------------------------------------------- - int numKinematicConstraints = - m_problem.getNumKinematicConstraintEquations(); - m_constraints.kinematic = MX( - casadi::Sparsity::dense(numKinematicConstraints, m_numMeshPoints)); + int nqerr = m_problem.getNumQErr(); + int nuerr = m_problem.getNumUErr(); + int nudoterr = m_problem.getNumUDotErr(); + int nkc = m_problem.getNumKinematicConstraintEquations(); const auto& kcBounds = m_problem.getKinematicConstraintBounds(); - m_constraintsLowerBounds.kinematic = casadi::DM::repmat( - kcBounds.lower, numKinematicConstraints, m_numMeshPoints); - m_constraintsUpperBounds.kinematic = casadi::DM::repmat( - kcBounds.upper, numKinematicConstraints, m_numMeshPoints); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nqerr + nuerr, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nqerr + nuerr, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nqerr + nuerr, m_numMeshPoints); + + m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( + nudoterr, m_numUDotErrorPoints)); + m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.lower, nudoterr, m_numUDotErrorPoints); + m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.upper, nudoterr, m_numUDotErrorPoints); + } else { + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nkc, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nkc, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nkc, m_numMeshPoints); + } + + // Initialize memory for projection constraints. + // --------------------------------------------- + int numProjectionConstraints = + m_problem.getNumProjectionConstraintEquations(); + m_constraints.projection = MX( + casadi::Sparsity::dense(numProjectionConstraints, + m_numMeshIntervals)); + m_constraintsLowerBounds.projection = + DM::zeros(numProjectionConstraints, m_numMeshIntervals); + m_constraintsUpperBounds.projection = + DM::zeros(numProjectionConstraints, m_numMeshIntervals); // qdot // ---- const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); m_xdot(Slice(0, NQ), Slice()) = u; + if (m_numProjectionStates) { + const MX u_projection = m_unscaledVars[projection_states] + (Slice(NQ, NQ + NU), Slice()); + m_xdot_projection(Slice(0, NQ), Slice()) = u_projection; + } - if (m_problem.getEnforceConstraintDerivatives() && - m_numMeshInteriorPoints && - !m_problem.isPrescribedKinematics()) { - // In Hermite-Simpson, we must compute a velocity correction at all mesh - // interval midpoints and update qdot. See MocoCasADiVelocityCorrection - // for more details. This function only takes multibody state variables: - // coordinates and speeds. - // TODO: The points at which we apply the velocity correction - // are correct for Trapezoidal (no points) and Hermite-Simpson (mesh - // interval midpoints), but might not be correct in general. Revisit - // this if we add other transcription schemes. - const auto velocityCorrectionOut = evalOnTrajectory( - m_problem.getVelocityCorrection(), {multibody_states, slacks}, - m_meshInteriorIndices); - const auto u_correction = velocityCorrectionOut.at(0); - - m_xdot(Slice(0, NQ), m_meshInteriorIndices) += u_correction; + // projection constraints + // ---------------------- + if (m_problem.getNumKinematicConstraintEquations() && + !m_problem.isPrescribedKinematics() && + m_problem.getEnforceConstraintDerivatives()) { + + OPENSIM_THROW_IF(!m_problem.isKinematicConstraintMethodBordalba2023() && + m_solver.getTranscriptionScheme() != "hermite-simpson", + OpenSim::Exception, + "Expected the 'hermite-simpson' transcription scheme when using " + "the Posa et al. 2016 method for enforcing kinematic constraints, " + "but the '{}' scheme was selected.", + m_solver.getTranscriptionScheme()); + + if (m_solver.getTranscriptionScheme() == "hermite-simpson" && + !m_problem.isKinematicConstraintMethodBordalba2023()) { + // In Hermite-Simpson, we must compute a velocity correction at all + // mesh interval midpoints and update qdot. + // See MocoCasADiVelocityCorrection for more details. This function + // only takes multibody state variables: coordinates and speeds. + const auto velocityCorrectionOut = evalOnTrajectory( + m_problem.getVelocityCorrection(), + {multibody_states, slacks}, + m_meshInteriorIndices); + const auto u_correction = velocityCorrectionOut.at(0); + + m_xdot(Slice(0, NQ), m_meshInteriorIndices) += u_correction; + } + + if (m_numProjectionStates) { + const auto projectionOut = evalOnTrajectory( + m_problem.getStateProjection(), + {multibody_states, slacks}, + m_projectionStateIndices); + const auto x_proj = projectionOut.at(0); + + const MX x = m_unscaledVars[states](Slice(0, NQ + NU), + m_projectionStateIndices); + const MX x_prime = m_unscaledVars[projection_states]; + + // Bordalba et al. (2023) projection equation: x = x' + F_x^T mu. + m_constraints.projection = x - x_prime - x_proj; + } } // udot, zdot, residual, kcerr // --------------------------- if (m_problem.isDynamicsModeImplicit()) { // udot. - const MX w = m_unscaledVars[derivatives](Slice(0, m_problem.getNumSpeeds()), - Slice()); + const MX w = m_unscaledVars[derivatives] + (Slice(0, m_problem.getNumSpeeds()), Slice()); m_xdot(Slice(NQ, NQ + NU), Slice()) = w; + if (m_numProjectionStates) { + const MX w_projection = m_unscaledVars[derivatives] + (Slice(0, m_problem.getNumSpeeds()), + m_projectionStateIndices); + m_xdot_projection(Slice(NQ, NQ + NU), Slice()) = w_projection; + } std::vector inputs{states, controls, multipliers, derivatives}; @@ -398,14 +564,28 @@ void Transcription::transcribe() { m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = out.at(2); - m_constraints.kinematic = out.at(3); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( + Slice(0, nqerr), Slice()); + m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) + = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); + m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = + out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } else { + m_constraints.kinematic = out.at(3); + } } // Points where we ignore algebraic constraints. if (m_numMeshInteriorPoints) { - const auto out = evalOnTrajectory( - m_problem.getImplicitMultibodySystemIgnoringConstraints(), - inputs, m_meshInteriorIndices); + const casadi::Function& implicitMultibodyFunction = + m_problem.isKinematicConstraintMethodBordalba2023() ? + m_problem.getImplicitMultibodySystem() : + m_problem.getImplicitMultibodySystemIgnoringConstraints(); + const auto out = evalOnTrajectory(implicitMultibodyFunction, inputs, + m_meshInteriorIndices); m_constraints.multibody_residuals(Slice(), m_meshInteriorIndices) = out.at(0); // zdot. @@ -413,6 +593,34 @@ void Transcription::transcribe() { out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) + = out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } + + // Points where the multibody residuals depend on the projection states. + if (m_numProjectionStates) { + const casadi::Function& implicitMultibodyFunction = + m_problem.getImplicitMultibodySystem(); + const auto out = evalOnTrajectory(implicitMultibodyFunction, + {projection_states, controls, multipliers, derivatives}, + m_projectionStateIndices); + // This overrides the previous function evaluation assignments for + // `kinematic_udoterr` and `multibody_residuals` at the mesh indices + // (i.e., for "points where we compute algebraic constraints"). We + // still need the function evaluations above since we need state + // derivatives for auxiliary states on the mesh points (note that + // the projection states overlap with all the mesh indices except + // the first mesh point). This also keeps the implementation above + // general when we do not have projection states. + m_constraints.multibody_residuals( + Slice(), m_projectionStateIndices) = out.at(0); + m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) + = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); } } else { // Explicit dynamics mode. @@ -429,13 +637,30 @@ void Transcription::transcribe() { m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = out.at(2); - m_constraints.kinematic = out.at(3); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( + Slice(0, nqerr), Slice()); + m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) + = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); + m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = + out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } else { + m_constraints.kinematic = out.at(3); + } } // Points where we ignore algebraic constraints. if (m_numMeshInteriorPoints) { - const auto out = evalOnTrajectory( - m_problem.getMultibodySystemIgnoringConstraints(), inputs, + // In the Bordalba et al. 2023 method, we need to enforce the + // acceleration-level kinematic constraints at the mesh interval + // interior points. + const casadi::Function& multibodyFunction = + m_problem.isKinematicConstraintMethodBordalba2023() ? + m_problem.getMultibodySystem() : + m_problem.getMultibodySystemIgnoringConstraints(); + const auto out = evalOnTrajectory(multibodyFunction, inputs, m_meshInteriorIndices); m_xdot(Slice(NQ, NQ + NU), m_meshInteriorIndices) = out.at(0); @@ -443,7 +668,69 @@ void Transcription::transcribe() { out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) + = out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } + + // Points where the state derivatives depend on the projection states. + if (m_numProjectionStates) { + const auto out = evalOnTrajectory(m_problem.getMultibodySystem(), + {projection_states, controls, multipliers, derivatives}, + m_projectionStateIndices); + // This state derivative is not used by Legendre-Gauss collocation + // since there is no collocation point at mesh interval endpoint. + // However, we need this function evaluation, since we still enforce + // the acceleration-level kinematic constraints at the mesh interval + // endpoints. + m_xdot_projection(Slice(NQ, NQ + NU), Slice()) = out.at(0); + // This overrides the previous function evaluation assignments for + // `kinematic_udoterr` at the mesh indices (i.e., for "points where + // we compute algebraic constraints"). We still need the function + // evaluations above since we need state derivatives for auxiliary + // states on the mesh points (note that the projection states + // overlap with all the mesh indices except the first mesh point). + // This also keeps the implementation above general when we do not + // have projection states. + m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) + = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } + + casadi_int istart = 0; + int numStates = m_problem.getNumStates(); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + casadi_int numPts = m_numPointsPerMeshInterval; + casadi_int iend = istart + numPts - 1; + if (m_numProjectionStates) { + // The state derivatives at all points in the mesh interval except + // the last point are the regular state derivatives. + m_stateDerivativesByMeshInterval[imesh] + (Slice(), Slice(0, numPts-1)) = + m_xdot(Slice(), Slice(istart, iend)); + + // The multibody state derivatives at the last point in the mesh + // interval are the projection state derivatives. + m_stateDerivativesByMeshInterval[imesh] + (Slice(0, m_numProjectionStates), numPts-1) = + m_xdot_projection(Slice(), imesh); + + // The non-multibody state derivatives at the last point (i.e., + // auxiliary state derivatives for muscles) are also the same as the + // regular state derivatives (there are no projection derivatives + // for these variables). + m_stateDerivativesByMeshInterval[imesh]( + Slice(m_numProjectionStates, numStates), numPts-1) = + m_xdot(Slice(m_numProjectionStates, numStates), iend); + } else { + m_stateDerivativesByMeshInterval[imesh](Slice(), Slice()) = + m_xdot(Slice(), Slice(istart, iend+1)); } + istart = iend; } // Calculate defects. @@ -513,6 +800,13 @@ void Transcription::setObjectiveAndEndpointConstraints() { if (minimizeAuxiliaryDerivatives) { m_objectiveTermNames.push_back("auxiliary_derivatives"); } + bool minimizeStateProjection = + m_solver.getMinimizeStateProjection() && + m_numProjectionStates && + m_problem.isKinematicConstraintMethodBordalba2023(); + if (minimizeStateProjection) { + m_objectiveTermNames.push_back("state_projection"); + } m_objectiveTerms = MX::zeros((int)m_objectiveTermNames.size(), 1); int iterm = 0; @@ -547,7 +841,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), - m_unscaledVars[parameters], + m_unscaledVars[parameters], integral}, costOut); m_objectiveTerms(iterm++) = casadi::MX::sum1(costOut.at(0)); @@ -566,7 +860,8 @@ void Transcription::setObjectiveAndEndpointConstraints() { // Minimize generalized accelerations. if (minimizeAccelerations) { const auto& numAccels = m_problem.getNumAccelerations(); - const auto accels = m_scaledVars[derivatives](Slice(0, numAccels), Slice()); + const auto accels = m_scaledVars[derivatives]( + Slice(0, numAccels), Slice()); const double accelWeight = m_solver.getImplicitMultibodyAccelerationsWeight(); MX integrandTraj = MX::sum1(MX::sq(accels)); @@ -587,6 +882,11 @@ void Transcription::setObjectiveAndEndpointConstraints() { dot(quadCoeffs.T(), integrandTraj); } + // Minimize state projection distance. + if (minimizeStateProjection && m_numProjectionStates) { + m_objectiveTerms(iterm++) = m_solver.getStateProjectionWeight() * + MX::sum2(MX::sum1(MX::sq(m_projectionStateDistances))); + } // Endpoint constraints // -------------------- @@ -640,7 +940,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { // ------------------- const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), guessOrig.variables.at(final_time)); - auto guess = guessOrig.resample(guessTimes); + bool appendProjectionStates = + m_problem.getNumProjectionConstraintEquations(); + auto guess = guessOrig.resample(guessTimes, appendProjectionStates); // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). @@ -653,9 +955,18 @@ Solution Transcription::solve(const Iterate& guessOrig) { if (slacks.size2() == m_numGridPoints) { casadi::DM meshIndices = createMeshIndices(); std::vector slackColumnsToRemove; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + slackColumnsToRemove.push_back(0); + } for (int itime = 0; itime < m_numGridPoints; ++itime) { - if (meshIndices(itime).__nonzero__()) { - slackColumnsToRemove.push_back(itime); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + if (!meshIndices(itime).__nonzero__()) { + slackColumnsToRemove.push_back(itime); + } + } else { + if (meshIndices(itime).__nonzero__()) { + slackColumnsToRemove.push_back(itime); + } } } // The first argument is an empty vector since we don't want to @@ -666,11 +977,44 @@ Solution Transcription::solve(const Iterate& guessOrig) { // Check that either that the slack variables provided in the guess // are the correct length, or that the correct number of columns // were removed. - OPENSIM_THROW_IF(slacks.size2() != m_numMeshInteriorPoints, + int slackLength; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + slackLength = m_numMeshIntervals; + } else { + slackLength = m_numMeshInteriorPoints; + } + OPENSIM_THROW_IF(slacks.size2() != slackLength, OpenSim::Exception, "Expected slack variables to be length {}, but they are length " "{}.", - m_numMeshInteriorPoints, slacks.size2()); + slackLength, slacks.size2()); + } + + // Adjust guesses for the projection variables to ensure they are the + // correct length. + if (guess.variables.find(Var::projection_states) != guess.variables.end()) { + auto& projection_states = guess.variables.at(Var::projection_states); + + if (projection_states.size2() == m_numGridPoints) { + casadi::DM meshIndices = createMeshIndices(); + std::vector columnsToRemove; + columnsToRemove.push_back(0); + + for (int itime = 0; itime < m_numGridPoints; ++itime) { + if (!meshIndices(itime).__nonzero__()) { + columnsToRemove.push_back(itime); + } + } + // The first argument is an empty vector since we don't want to + // remove an entire row. + projection_states.remove(std::vector(), columnsToRemove); + } + + OPENSIM_THROW_IF(projection_states.size2() != m_numMeshIntervals, + OpenSim::Exception, + "Expected projection state variables to be length {}, but they " + "are length {}.", + m_numMeshIntervals, projection_states.size2()); } // Create the CasADi NLP function. @@ -1030,19 +1374,25 @@ void Transcription::printConstraintValues(const Iterate& it, ss << "\nKinematic constraints:"; std::vector kinconNames = m_problem.createKinematicConstraintEquationNames(); + int numQErr = m_problem.getNumQErr(); + int numUErr = m_problem.getNumUErr(); + int numUDotErr = m_problem.getNumUDotErr(); + int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? + numQErr + numUErr : numQErr + numUErr + numUDotErr; if (kinconNames.empty()) { ss << " none" << std::endl; } else { maxNameLength = 0; updateMaxNameLength(kinconNames); ss << "\n L2 norm across mesh, max abs value (L1 norm), time of " - "max " - "abs" - << std::endl; + "max " + "abs" + << std::endl; row.resize(1, m_numMeshPoints); { - for (int ikc = 0; ikc < (int)constraints.kinematic.rows(); ++ikc) { - row = constraints.kinematic(ikc, Slice()); + int ikc = 0; + for (int i = 0; i < numKC; ++i) { + row = constraints.kinematic(i, Slice()); const double L2 = casadi::DM::norm_2(row).scalar(); int argmax; double max = calcL1Norm(row, argmax); @@ -1051,15 +1401,35 @@ void Transcription::printConstraintValues(const Iterate& it, std::string label = kinconNames.at(ikc); ss << std::setfill('0') << std::setw(2) << ikc << ":" - << std::setfill(' ') << std::setw(maxNameLength) << label - << spacer << std::setprecision(2) << std::scientific - << std::setw(9) << L2 << spacer << L1 << spacer - << std::setprecision(6) << std::fixed << time_of_max - << std::endl; + << std::setfill(' ') << std::setw(maxNameLength) << label + << spacer << std::setprecision(2) << std::scientific + << std::setw(9) << L2 << spacer << L1 << spacer + << std::setprecision(6) << std::fixed << time_of_max + << std::endl; + ++ikc; + } + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + row = constraints.kinematic_udoterr(iudot, Slice()); + const double L2 = casadi::DM::norm_2(row).scalar(); + int argmax; + double max = calcL1Norm(row, argmax); + const double L1 = max; + const double time_of_max = it.times(argmax).scalar(); + + std::string label = kinconNames.at(ikc); + ss << std::setfill('0') << std::setw(2) << ikc << ":" + << std::setfill(' ') << std::setw(maxNameLength) << label + << spacer << std::setprecision(2) << std::scientific + << std::setw(9) << L2 << spacer << L1 << spacer + << std::setprecision(6) << std::fixed << time_of_max + << std::endl; + ++ikc; + } } } ss << "Kinematic constraint values at each mesh point:" - << std::endl; + << std::endl; ss << " time "; for (int ipc = 0; ipc < (int)kinconNames.size(); ++ipc) { ss << std::setw(9) << ipc << " "; @@ -1069,10 +1439,19 @@ void Transcription::printConstraintValues(const Iterate& it, ss << std::setfill('0') << std::setw(3) << imesh << " "; ss.fill(' '); ss << std::setw(9) << it.times(imesh).scalar() << " "; - for (int ikc = 0; ikc < (int)kinconNames.size(); ++ikc) { - const auto& value = constraints.kinematic(ikc, imesh).scalar(); + for (int i = 0; i < numKC; ++i) { + const auto& value = + constraints.kinematic(i, imesh).scalar(); ss << std::setprecision(2) << std::scientific - << std::setw(9) << value << " "; + << std::setw(9) << value << " "; + } + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + const auto& value = + constraints.kinematic_udoterr(iudot, imesh).scalar(); + ss << std::setprecision(2) << std::scientific + << std::setw(9) << value << " "; + } } ss << std::endl; } @@ -1237,12 +1616,22 @@ casadi::MXVector Transcription::evalOnTrajectory( // Add 1 for time input and 1 for parameters input. MXVector mxIn(inputs.size() + 2); mxIn[0] = m_times(timeIndices); + const auto NQ = m_problem.getNumCoordinates(); + const auto NU = m_problem.getNumSpeeds(); + const auto NS = m_problem.getNumStates(); for (int i = 0; i < (int)inputs.size(); ++i) { if (inputs[i] == multibody_states) { - const auto NQ = m_problem.getNumCoordinates(); - const auto NU = m_problem.getNumSpeeds(); - mxIn[i + 1] = - m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); + mxIn[i + 1] = + m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); + } else if (inputs[i] == projection_states) { + const auto& proj_states = m_unscaledVars.at(projection_states); + OPENSIM_ASSERT(proj_states.size2() == timeIndices.size2()); + mxIn[i + 1] = casadi::MX(NS, timeIndices.size2()); + mxIn[i + 1](Slice(0, NQ + NU), Slice()) = + m_unscaledVars.at(projection_states)(Slice(0, NQ + NU), + Slice()); + mxIn[i + 1](Slice(NQ + NU, NS), Slice()) = + m_unscaledVars.at(states)(Slice(NQ + NU, NS), timeIndices); } else if (inputs[i] == slacks) { mxIn[i + 1] = m_unscaledVars.at(inputs[i]); } else { @@ -1257,6 +1646,8 @@ casadi::MXVector Transcription::evalOnTrajectory( mxIn[mxIn.size() - 1] = m_paramsTrajMeshInterior; } else if (&timeIndices == &m_pathConstraintIndices) { mxIn[mxIn.size() - 1] = m_paramsTrajPathCon; + } else if (&timeIndices == &m_projectionStateIndices) { + mxIn[mxIn.size() - 1] = m_paramsTrajProjState; } else { OPENSIM_THROW(OpenSim::Exception, "Internal error."); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 3e13b38ffb..0cab17732c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -3,9 +3,9 @@ /* -------------------------------------------------------------------------- * * OpenSim: CasOCTranscription.h * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -73,6 +73,7 @@ class Transcription { /// scheme applies constraints between control points. void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numPointsPerMeshInterval, const casadi::DM& pointsForInterpControls = casadi::DM()); /// We assume all functions depend on time and parameters. @@ -127,9 +128,11 @@ class Transcription { T multibody_residuals; T auxiliary_residuals; T kinematic; + T kinematic_udoterr; std::vector endpoint; std::vector path; T interp_controls; + T projection; }; void printConstraintValues(const Iterate& it, const Constraints& constraints, @@ -145,10 +148,13 @@ class Transcription { int m_numMeshIntervals = -1; int m_numMeshInteriorPoints = -1; int m_numDefectsPerMeshInterval = -1; + int m_numPointsPerMeshInterval = -1; + int m_numUDotErrorPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; + int m_numProjectionStates = -1; casadi::DM m_grid; casadi::DM m_pointsForInterpControls; casadi::MX m_times; @@ -161,18 +167,34 @@ class Transcription { casadi::MX m_paramsTrajMesh; casadi::MX m_paramsTrajMeshInterior; casadi::MX m_paramsTrajPathCon; + casadi::MX m_paramsTrajProjState; VariablesDM m_lowerBounds; VariablesDM m_upperBounds; VariablesDM m_shift; VariablesDM m_scale; + // These hold vectors of MX types, where each element of the vector + // contains either the states or state derivatives needed to calculate the + // defect constraints for a single mesh interval. + casadi::MXVector m_statesByMeshInterval; + casadi::MXVector m_stateDerivativesByMeshInterval; casadi::DM m_meshIndicesMap; casadi::Matrix m_gridIndices; casadi::Matrix m_meshIndices; casadi::Matrix m_meshInteriorIndices; casadi::Matrix m_pathConstraintIndices; + casadi::Matrix m_projectionStateIndices; + casadi::Matrix m_notProjectionStateIndices; + + // State derivatives. + casadi::MX m_xdot; + // State derivatives reserved for the Bordalba et al. (2023) kinematic + // constraint method based on coordinate projection. + casadi::MX m_xdot_projection; + // The differences between the true states and the projection states when + // using the Bordalba et al. (2023) kinematic constraint method. + casadi::MX m_projectionStateDistances; - casadi::MX m_xdot; // State derivatives. casadi::MX m_objectiveTerms; std::vector m_objectiveTermNames; @@ -193,8 +215,8 @@ class Transcription { virtual casadi::DM createMeshIndicesImpl() const = 0; /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. - virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const = 0; + virtual void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const = 0; virtual void calcInterpolatingControlsImpl(const casadi::MX& /*controls*/, casadi::MX& /*interpControls*/) const { OPENSIM_THROW_IF(m_pointsForInterpControls.numel(), OpenSim::Exception, @@ -204,8 +226,8 @@ class Transcription { void transcribe(); void setObjectiveAndEndpointConstraints(); void calcDefects() { - calcDefectsImpl( - m_unscaledVars.at(states), m_xdot, m_constraints.defects); + calcDefectsImpl(m_statesByMeshInterval, + m_stateDerivativesByMeshInterval, m_constraints.defects); } void calcInterpolatingControls() { calcInterpolatingControlsImpl( @@ -307,82 +329,135 @@ class Transcription { // Trapezoidal sparsity pattern (mapping between flattened and expanded // constraints) for mesh intervals 0, 1 and 2: Endpoint constraints // depend on all time points through their integral. + // // 0 1 2 3 // endpoint x x x x // path_0 x // kinematic_0 x // residual_0 x // defect_0 x x - // kinematic_1 x + // projection_1 x // path_1 x + // kinematic_1 x // residual_1 x // defect_1 x x - // kinematic_2 x + // projection_2 x // path_2 x + // kinematic_2 x // residual_2 x - // kinematic_3 x + // defect_2 x x + // projection_3 x // path_3 x + // kinematic_3 x // residual_3 x - // Hermite-Simpson sparsity pattern for mesh intervals 0, 1 and 2 - // (* indicates additional non-zero entry when path constraint - // midpoints are enforced): - // 0 0.5 1 1.5 2 2.5 3 - // endpoint x x x x x x x - // path_0 x * - // kinematic_0 x - // residual_0 x - // residual_0.5 x - // defect_0 x x x - // interp_con_0 x x x - // kinematic_1 x - // path_1 x * - // residual_1 x - // residual_1.5 x - // defect_1 x x x - // interp_con_1 x x x - // kinematic_2 x - // path_2 x * - // residual_2 x - // residual_2.5 x - // defect_2 x x x - // interp_con_2 x x x - // kinematic_3 x - // path_3 x - // residual_3 x - // 0 0.5 1 1.5 2 2.5 3 + // Hermite-Simpson sparsity pattern for mesh intervals 0, 1 and 2. + // '*' indicates additional non-zero entry when path constraint + // mesh interior points are enforced. Note that acceleration-level + // kinematic constraints, "kinematic_udoterr_0", are only enforced at + // mesh interior points (e.g., 0.5, 1.5, 2.5) when using the Bordalba + // et al. (2023) kinematic constraint method. This sparsity pattern also + // applies to the Legendre-Gauss and Legendre-Gauss-Radau transcription + // with polynomial degree equal to 1. + // + // 0 0.5 1 1.5 2 2.5 3 + // endpoint x x x x x x x + // path_0 x * + // kinematic_perr_0 x + // kinematic_uerr_0 x + // kinematic_udoterr_0 x x + // residual_0 x x + // defect_0 x x x + // interp_con_0 x x x + // projection_1 x + // path_1 x * + // kinematic_perr_1 x + // kinematic_uerr_1 x + // kinematic_udoterr_1 x x + // residual_1 x x + // defect_1 x x x + // interp_con_1 x x x + // projection_2 x + // path_2 x * + // kinematic_perr_2 x + // kinematic_uerr_2 x + // kinematic_udoterr_2 x x + // residual_2 x x + // defect_2 x x x + // interp_con_2 x x x + // projection_3 x + // path_3 x + // kinematic_perr_3 x + // kinematic_uerr_3 x + // kinematic_udoterr_3 x + // residual_3 x + // 0 0.5 1 1.5 2 2.5 3 for (const auto& endpoint : constraints.endpoint) { copyColumn(endpoint, 0); } - for (int ipc = 0; ipc < m_numPathConstraintPoints; ++ipc) { - for (const auto& path: constraints.path) { - copyColumn(path, ipc); + // Constraints for each mesh interval. + int N = m_numPointsPerMeshInterval - 1; + int icon = 0; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + + // Path constraints. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (int i = 0; i < N; ++i) { + for (const auto& path : constraints.path) { + copyColumn(path, igrid + i); + } + } + } else { + for (const auto& path : constraints.path) { + copyColumn(path, imesh); + } } - } - int igrid = 0; - // Index for pointsForInterpControls. - int icon = 0; - for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + // Kinematic constraints. copyColumn(constraints.kinematic, imesh); - if (imesh < m_numMeshIntervals) { - while (m_grid(igrid).scalar() < m_solver.getMesh()[imesh + 1]) { - copyColumn(constraints.multibody_residuals, igrid); - copyColumn(constraints.auxiliary_residuals, igrid); - ++igrid; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int i = 0; i < N; ++i) { + copyColumn(constraints.kinematic_udoterr, igrid + i); } - copyColumn(constraints.defects, imesh); - while (icon < m_pointsForInterpControls.numel() && - m_pointsForInterpControls(icon).scalar() < - m_solver.getMesh()[imesh + 1]) { - copyColumn(constraints.interp_controls, icon); - ++icon; + } + + // Multibody and auxiliary residuals. + for (int i = 0; i < N; ++i) { + copyColumn(constraints.multibody_residuals, igrid + i); + copyColumn(constraints.auxiliary_residuals, igrid + i); + } + + // Defect constraints. + copyColumn(constraints.defects, imesh); + + // Interpolating controls. + if (m_pointsForInterpControls.numel()) { + for (int i = 0; i < N-1; ++i) { + copyColumn(constraints.interp_controls, icon++); } } + + // Projection constraints. + copyColumn(constraints.projection, imesh); + } + + // Final grid point. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (const auto& path : constraints.path) { + copyColumn(path, m_numGridPoints - 1); + } + } else { + for (const auto& path : constraints.path) { + copyColumn(path, m_numMeshPoints - 1); + } + } + copyColumn(constraints.kinematic, m_numMeshPoints - 1); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(constraints.kinematic_udoterr, m_numGridPoints - 1); } - // The loop above does not handle the residual at the final grid point. copyColumn(constraints.multibody_residuals, m_numGridPoints - 1); copyColumn(constraints.auxiliary_residuals, m_numGridPoints - 1); @@ -403,12 +478,21 @@ class Transcription { }; Constraints out; out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); - out.multibody_residuals = init(m_numMultibodyResiduals, + out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); out.auxiliary_residuals = init(m_numAuxiliaryResiduals, m_numGridPoints); - out.kinematic = init(m_problem.getNumKinematicConstraintEquations(), - m_numMeshPoints); + int numQErr = m_problem.getNumQErr(); + int numUErr = m_problem.getNumUErr(); + int numUDotErr = m_problem.getNumUDotErr(); + int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? + numQErr + numUErr : numQErr + numUErr + numUDotErr; + out.kinematic = init(numKC,m_numMeshPoints); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + out.kinematic_udoterr = init(numUDotErr, m_numUDotErrorPoints); + } + out.projection = init(m_problem.getNumProjectionConstraintEquations(), + m_numMeshIntervals); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; @@ -436,33 +520,67 @@ class Transcription { copyColumn(endpoint, 0); } - for (int ipc = 0; ipc < m_numPathConstraintPoints; ++ipc) { - for (auto& path : out.path) { - copyColumn(path, ipc); + // Constraints for each mesh interval. + int N = m_numPointsPerMeshInterval - 1; + int icon = 0; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + + // Path constraints. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (int i = 0; i < N; ++i) { + for (auto& path : out.path) { + copyColumn(path, igrid + i); + } + } + } else { + for (auto& path : out.path) { + copyColumn(path, imesh); + } } - } - int igrid = 0; - // Index for pointsForInterpControls. - int icon = 0; - for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + // Kinematic constraints. copyColumn(out.kinematic, imesh); - if (imesh < m_numMeshIntervals) { - while (m_grid(igrid).scalar() < m_solver.getMesh()[imesh + 1]) { - copyColumn(out.multibody_residuals, igrid); - copyColumn(out.auxiliary_residuals, igrid); - ++igrid; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int i = 0; i < N; ++i) { + copyColumn(out.kinematic_udoterr, igrid + i); } - copyColumn(out.defects, imesh); - while (icon < m_pointsForInterpControls.numel() && - m_pointsForInterpControls(icon).scalar() < - m_solver.getMesh()[imesh + 1]) { - copyColumn(out.interp_controls, icon); - ++icon; + } + + // Multibody and auxiliary residuals. + for (int i = 0; i < N; ++i) { + copyColumn(out.multibody_residuals, igrid + i); + copyColumn(out.auxiliary_residuals, igrid + i); + } + + // Defect constraints. + copyColumn(out.defects, imesh); + + // Interpolating controls. + if (m_pointsForInterpControls.numel()) { + for (int i = 0; i < N-1; ++i) { + copyColumn(out.interp_controls, icon++); } } + + // Projection constraints. + copyColumn(out.projection, imesh); + } + + // Final grid point. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (auto& path : out.path) { + copyColumn(path, m_numGridPoints - 1); + } + } else { + for (auto& path : out.path) { + copyColumn(path, m_numMeshPoints - 1); + } + } + copyColumn(out.kinematic, m_numMeshPoints - 1); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(out.kinematic_udoterr, m_numGridPoints - 1); } - // The loop above does not handle residuals at the final grid point. copyColumn(out.multibody_residuals, m_numGridPoints - 1); copyColumn(out.auxiliary_residuals, m_numGridPoints - 1); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index a3834c2ea4..4a9a644565 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -40,18 +40,18 @@ DM Trapezoidal::createMeshIndicesImpl() const { return DM::ones(1, m_numGridPoints); } -void Trapezoidal::calcDefectsImpl( - const casadi::MX& x, const casadi::MX& xdot, casadi::MX& defects) const { +void Trapezoidal::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian // this way might have benefits for sparse linear algebra). for (int itime = 0; itime < m_numMeshIntervals; ++itime) { const auto h = m_times(itime + 1) - m_times(itime); - const auto x_i = x(Slice(), itime); - const auto x_ip1 = x(Slice(), itime + 1); - const auto xdot_i = xdot(Slice(), itime); - const auto xdot_ip1 = xdot(Slice(), itime + 1); + const auto x_i = x[itime](Slice(), 0); + const auto x_ip1 = x[itime](Slice(), 1); + const auto xdot_i = xdot[itime](Slice(), 0); + const auto xdot_ip1 = xdot[itime](Slice(), 1); // Trapezoidal defects. defects(Slice(), itime) = x_ip1 - (x_i + 0.5 * h * (xdot_ip1 + xdot_i)); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index b44367bc24..984790dc61 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -29,21 +29,17 @@ class Trapezoidal : public Transcription { public: Trapezoidal(const Solver& solver, const Problem& problem) : Transcription(solver, problem) { - - OPENSIM_THROW_IF(problem.getEnforceConstraintDerivatives(), - OpenSim::Exception, - "Enforcing kinematic constraint derivatives " - "not supported with trapezoidal transcription."); createVariablesAndSetBounds(m_solver.getMesh(), - m_problem.getNumStates()); + m_problem.getNumStates(), 2); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, + casadi::MX& defects) const override; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 1eca2adc09..d83f461d4c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -54,7 +54,10 @@ void MocoCasADiSolver::constructProperties() { constructProperty_minimize_implicit_auxiliary_derivatives(false); constructProperty_implicit_auxiliary_derivatives_weight(1.0); - constructProperty_enforce_path_constraint_midpoints(false); + constructProperty_enforce_path_constraint_mesh_interior_points(false); + constructProperty_minimize_state_projection_distance(true); + constructProperty_state_projection_distance_weight(1e-6); + constructProperty_projection_slack_variable_bounds({-1e-3, 1e-3}); } bool MocoCasADiSolver::isAvailable() { @@ -79,11 +82,11 @@ MocoTrajectory MocoCasADiSolver::createGuess(const std::string& type) const { auto casProblem = createCasOCProblem(); auto casSolver = createCasOCSolver(*casProblem); - std::vector inputControlIndexes = + std::vector inputControlIndexes = getProblemRep().getInputControlIndexes(); if (type == "bounds") { return convertToMocoTrajectory( - casSolver->createInitialGuessFromBounds(), + casSolver->createInitialGuessFromBounds(), inputControlIndexes); } else if (type == "random") { return convertToMocoTrajectory( @@ -180,8 +183,20 @@ std::unique_ptr MocoCasADiSolver::createCasOCProblem() const { OPENSIM_THROW_IF(!model.getMatterSubsystem().getUseEulerAngles( model.getWorkingState()), Exception, "Quaternions are not supported."); + + if (getProblemRep().getNumKinematicConstraintEquations()) { + checkPropertyValueIsInSet(getProperty_kinematic_constraint_method(), + {"Posa2016", "Bordalba2023"}); + OPENSIM_THROW_IF(get_transcription_scheme() != "hermite-simpson" && + get_kinematic_constraint_method() == "Posa2016", Exception, + "Expected the 'hermite-simpson' transcription scheme when using " + "the Posa et al. 2016 method for enforcing kinematic constraints, " + "but received '{}'.", get_transcription_scheme()); + } + return OpenSim::make_unique(*this, problemRep, - createProblemRepJar(numThreads), get_multibody_dynamics_mode()); + createProblemRepJar(numThreads), get_multibody_dynamics_mode(), + get_kinematic_constraint_method()); #else OPENSIM_THROW(MocoCasADiSolverNotAvailable); #endif @@ -205,22 +220,6 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( "legendre-gauss-radau-4", "legendre-gauss-radau-5", "legendre-gauss-radau-6", "legendre-gauss-radau-7", "legendre-gauss-radau-8", "legendre-gauss-radau-9"}); - OPENSIM_THROW_IF(casProblem.getNumKinematicConstraintEquations() != 0 && - get_transcription_scheme() == "trapezoidal", - OpenSim::Exception, - "Kinematic constraints not supported with " - "trapezoidal transcription."); - // Enforcing constraint derivatives is not supported with trapezoidal - // transcription. - if (casProblem.getNumKinematicConstraintEquations() != 0) { - OPENSIM_THROW_IF(get_transcription_scheme() == "trapezoidal" && - get_enforce_constraint_derivatives(), - Exception, - "The current transcription scheme is '{}', but enforcing " - "kinematic constraint derivatives is not supported with " - "trapezoidal transcription.", - get_transcription_scheme()); - } checkPropertyValueIsInRangeOrSet(getProperty_num_mesh_intervals(), 0, std::numeric_limits::max(), {}); @@ -335,11 +334,15 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( casSolver->setImplicitAuxiliaryDerivativesWeight( get_implicit_auxiliary_derivatives_weight()); + casSolver->setMinimizeStateProjection( + get_minimize_state_projection_distance()); + casSolver->setStateProjectionWeight(get_state_projection_distance_weight()); + casSolver->setOptimSolver(get_optim_solver()); - casSolver->setInterpolateControlMidpoints( - get_interpolate_control_midpoints()); - casSolver->setEnforcePathConstraintMidpoints( - get_enforce_path_constraint_midpoints()); + casSolver->setInterpolateControlMeshInteriorPoints( + get_interpolate_control_mesh_interior_points()); + casSolver->setEnforcePathConstraintMeshInteriorPoints( + get_enforce_path_constraint_mesh_interior_points()); if (casProblem.getJarSize() > 1) { casSolver->setParallelism("thread", casProblem.getJarSize()); } @@ -368,14 +371,22 @@ MocoSolution MocoCasADiSolver::solveImpl() const { log_info("Number of threads: {}", casProblem->getJarSize()); } - std::vector inputControlIndexes = + std::vector inputControlIndexes = getProblemRep().getInputControlIndexes(); MocoTrajectory guess = getGuess(); CasOC::Iterate casGuess; if (guess.empty()) { casGuess = casSolver->createInitialGuessFromBounds(); } else { - casGuess = convertToCasOCIterate(guess, inputControlIndexes); + std::vector expectedSlackNames; + for (const auto& info : casProblem->getSlackInfos()) { + expectedSlackNames.push_back(info.name); + } + // We do not need to append projection states here since they will be + // appended later when the guess is resampled by the solver (if needed). + bool appendProjectionStates = false; + casGuess = convertToCasOCIterate(guess, expectedSlackNames, + appendProjectionStates, inputControlIndexes); } // Temporarily disable printing of negative muscle force warnings so the @@ -409,6 +420,7 @@ MocoSolution MocoCasADiSolver::solveImpl() const { !get_minimize_lagrange_multipliers()) { checkConstraintJacobianRank(mocoSolution); } + checkSlackVariables(mocoSolution); const long long elapsed = stopwatch.getElapsedTimeInNs(); setSolutionStats(mocoSolution, casSolution.stats.at("success"), diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h index d6bba90197..d741671474 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h @@ -172,10 +172,26 @@ class OSIMMOCO_API MocoCasADiSolver : public MocoDirectCollocationSolver { "'minimize_implicit_auxiliary_derivatives' is enabled." "Default: 1.0."); - OpenSim_DECLARE_PROPERTY(enforce_path_constraint_midpoints, bool, - "If the transcription scheme is set to 'hermite-simpson', then " - "enable this property to enforce MocoPathConstraints at mesh " - "interval midpoints. Default: false."); + OpenSim_DECLARE_PROPERTY(enforce_path_constraint_mesh_interior_points, bool, + "If the transcription scheme is set to 'hermite-simpson' or one of " + "the pseudospectral schemes (e.g., 'legendre-gauss-3', " + "'legendre-gauss-radau-3'), then enable this property to enforce " + "MocoPathConstraints at points interior to the mesh interval. " + "Default: false."); + + OpenSim_DECLARE_PROPERTY(minimize_state_projection_distance, bool, + "Minimize the distance between the projection states and the " + "constraint manifold when using the 'Bordalba2023' method for " + "enforcing kinematic constraints. Default: true."); + OpenSim_DECLARE_PROPERTY(state_projection_distance_weight, double, + "The weight on the cost term if " + "'minimize_state_projection_distance' is enabled. " + "Default: 1e-6."); + OpenSim_DECLARE_PROPERTY(projection_slack_variable_bounds, MocoBounds, + "The bounds on the slack variables added to the problem which " + "determine the magnitude of the constraint projection at each " + "mesh interval when using the 'Bordalba2023' method for " + "enforcing kinematic constraints. Default: [-1e-3, 1e-3]."); MocoCasADiSolver(); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 95525a0898..4a752d5cdb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -1,9 +1,9 @@ /* -------------------------------------------------------------------------- * * OpenSim Moco: MocoCasOCProblem.cpp * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -22,6 +22,8 @@ #include +#include + using namespace OpenSim; thread_local SimTK::Vector_ @@ -32,13 +34,14 @@ thread_local SimTK::Vector MocoCasOCProblem::m_pvaerr; MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, const MocoProblemRep& problemRep, std::unique_ptr> jar, - std::string dynamicsMode) + std::string dynamicsMode, std::string kinematicConstraintMethod) : m_jar(std::move(jar)), m_paramsRequireInitSystem( mocoCasADiSolver.get_parameters_require_initsystem()), m_formattedTimeString(getFormattedDateTime(true)) { - setDynamicsMode(dynamicsMode); + setDynamicsMode(std::move(dynamicsMode)); + setKinematicConstraintMethod(std::move(kinematicConstraintMethod)); const auto& model = problemRep.getModelBase(); if (problemRep.isPrescribedKinematics()) { @@ -78,6 +81,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // dynamics in implicit form. const auto& implicitRefs = problemRep.getImplicitComponentReferencePtrs(); std::vector derivativeNames; + derivativeNames.reserve(implicitRefs.size()); for (const auto& implicitRef : implicitRefs) { derivativeNames.push_back(implicitRef.second->getAbsolutePathString() + "/" + implicitRef.first); @@ -87,9 +91,8 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // Add any scalar constraints associated with kinematic constraints in // the model as path constraints in the problem. - // Whether or not enabled kinematic constraints exist in the model, - // check that optional solver properties related to constraints are - // set properly. + // Whether enabled kinematic constraints exist in the model, check that + // optional solver properties related to constraints are set properly. const auto kcNames = problemRep.createKinematicConstraintNames(); if (kcNames.empty()) { OPENSIM_THROW_IF(mocoCasADiSolver.get_minimize_lagrange_multipliers(), @@ -112,21 +115,26 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, ma = kc.getNumAccelerationEquations(); kinLevels = kc.getKinematicLevels(); - // TODO only add velocity correction variables for holonomic - // constraint derivatives? For now, disallow enforcing derivatives - // if non-holonomic or acceleration constraints present. - OPENSIM_THROW_IF(enforceConstraintDerivs && mv != 0, Exception, - "Enforcing constraint derivatives is supported only for " - "holonomic (position-level) constraints. There are {} " - "velocity-level scalar constraints associated with the " - "model Constraint at ConstraintIndex {}.", - mv, cid); - OPENSIM_THROW_IF(enforceConstraintDerivs && ma != 0, Exception, - "Enforcing constraint derivatives is supported only for " - "holonomic (position-level) constraints. There are {} " - "acceleration-level scalar constraints associated with the " - "model Constraint at ConstraintIndex {}.", - ma, cid); + if (isKinematicConstraintMethodBordalba2023()) { + OPENSIM_THROW_IF(!enforceConstraintDerivs, Exception, + "The Bordalba et al. 2023 method for enforcing " + "kinematic constraints requires that the solver " + "property 'enforce_constraint_derivatives' be set to " + "true."); + } else { + OPENSIM_THROW_IF(mv != 0, Exception, + "The Posa et al. 2016 method is not compatible with " + "velocity-level kinematic constraints. There are {} " + "velocity-level scalar constraints associated with " + "the model Constraint at ConstraintIndex {}.", + mv, cid); + OPENSIM_THROW_IF(ma != 0, Exception, + "The Posa et al. 2016 method is not compatible with " + "acceleration-level kinematic constraints. There are " + "{} acceleration-level scalar constraints associated " + "with the model Constraint at ConstraintIndex {}.", + ma, cid); + } // Loop through all scalar constraints associated with the model // constraint and corresponding path constraints to the optimal @@ -145,8 +153,8 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, kinLevels[i] == KinematicLevel::Velocity || kinLevels[i] == KinematicLevel::Acceleration) { + // Grab the constraint information from the MocoProblem. const auto& multInfo = multInfos[multIndexThisConstraint]; - CasOC::KinematicLevel kinLevel; if (kinLevels[i] == KinematicLevel::Position) kinLevel = CasOC::KinematicLevel::Position; @@ -156,20 +164,22 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, kinLevel = CasOC::KinematicLevel::Acceleration; else { OPENSIM_THROW(OpenSim::Exception, - "Unrecognized KinematicLevel"); + "Unrecognized KinematicLevel"); } + // This adds a Lagrange multiplier to the current problem, + // and, if kinematics are not prescribed, increments the + // number of scalar kinematic constraint equations added. addKinematicConstraint(multInfo.getName(), - convertBounds(multInfo.getBounds()), - convertBounds(multInfo.getInitialBounds()), - convertBounds(multInfo.getFinalBounds()), kinLevel); - - // Add velocity correction variables if enforcing - // constraint equation derivatives. - if (enforceConstraintDerivs && !isPrescribedKinematics()) { - // TODO this naming convention assumes that the - // associated Lagrange multiplier name begins with - // "lambda", which may change in the future. + convertBounds(multInfo.getBounds()), + convertBounds(multInfo.getInitialBounds()), + convertBounds(multInfo.getFinalBounds()), kinLevel); + + // If kinematics are not prescribed and we are enforcing + // kinematic constraints explicitly, we need to add + // additional "slack" variables to ensure the problem is not + // over constrained. + if (!isPrescribedKinematics()) { OPENSIM_THROW_IF( multInfo.getName().substr(0, 6) != "lambda", Exception, @@ -177,12 +187,39 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, "constraint to begin with 'lambda' but it " "begins with '{}'.", multInfo.getName().substr(0, 6)); - const auto vcBounds = convertBounds( + if (isKinematicConstraintMethodBordalba2023()) { + // Add "mu" variables for the projection method by + // Bordalba et al. (2023). + const auto muBounds = convertBounds( mocoCasADiSolver - .get_velocity_correction_bounds()); - addSlack(std::string(multInfo.getName()) - .replace(0, 6, "gamma"), - vcBounds); + .get_projection_slack_variable_bounds()); + if (kinLevel == CasOC::KinematicLevel::Position) { + // For holonomic constraints, we need to add + // two slack variables: one for the position + // error and one for the derivative of the + // position error. + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "mu"), + muBounds); + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "mu") + "_dot", + muBounds); + } else if (kinLevel == + CasOC::KinematicLevel::Velocity) { + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "mu"), + muBounds); + } + } else if (enforceConstraintDerivs) { + // Add "gamma" variables for the method by + // Posa et al. (2015). + const auto vcBounds = convertBounds( + mocoCasADiSolver + .get_velocity_correction_bounds()); + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "gamma"), + vcBounds); + } } ++multIndexThisConstraint; } @@ -191,6 +228,19 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // Set kinematic constraint information on the CasOC::Problem. setEnforceConstraintDerivatives(enforceConstraintDerivs); + + // Set the kinematic constraint offsets, if not enforcing the + // derivatives of the kinematic constraint equations. + m_uerrOffset = getEnforceConstraintDerivatives() ? 0 : + getNumHolonomicConstraintEquations(); + m_uerrSize = getEnforceConstraintDerivatives() ? getNumUErr() : + getNumNonHolonomicConstraintEquations(); + m_udoterrOffset = getEnforceConstraintDerivatives() ? 0 : + getNumHolonomicConstraintEquations() + + getNumNonHolonomicConstraintEquations(); + m_udoterrSize = getEnforceConstraintDerivatives() ? getNumUDotErr() : + getNumAccelerationConstraintEquations(); + // The bounds are the same for all kinematic constraints in the // MocoProblem, so just grab the bounds from the first constraint. // TODO: This behavior may be unexpected for users. diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 2667829f13..8032f357fb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -3,9 +3,9 @@ /* -------------------------------------------------------------------------- * * OpenSim: MocoCasOCProblem.h * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -67,35 +67,37 @@ inline casadi::DM convertToCasADiDM(const SimTK::Vector& simtkVec) { return convertToCasADiDMTemplate(simtkVec); } -/// This resamples the iterate to obtain values that lie on the mesh. -inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, +/// This converts a MocoTrajectory to a CasOC::Iterate. +inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoTraj, + const std::vector& expectedSlackNames, + bool appendProjectionStates = false, std::vector inputControlIndexes = {}) { CasOC::Iterate casIt; CasOC::VariablesDM& casVars = casIt.variables; using CasOC::Var; - casVars[Var::initial_time] = mocoIt.getInitialTime(); - casVars[Var::final_time] = mocoIt.getFinalTime(); + casVars[Var::initial_time] = mocoTraj.getInitialTime(); + casVars[Var::final_time] = mocoTraj.getFinalTime(); casVars[Var::states] = - convertToCasADiDMTranspose(mocoIt.getStatesTrajectory()); - - casadi::DM controls = - convertToCasADiDMTranspose(mocoIt.getControlsTrajectory()); - casadi::DM input_controls = - convertToCasADiDMTranspose(mocoIt.getInputControlsTrajectory()); - std::vector controlNames = mocoIt.getControlNames(); - std::vector inputControlNames = mocoIt.getInputControlNames(); + convertToCasADiDMTranspose(mocoTraj.getStatesTrajectory()); + + casadi::DM controls = + convertToCasADiDMTranspose(mocoTraj.getControlsTrajectory()); + casadi::DM input_controls = + convertToCasADiDMTranspose(mocoTraj.getInputControlsTrajectory()); + std::vector controlNames = mocoTraj.getControlNames(); + std::vector inputControlNames = mocoTraj.getInputControlNames(); std::vector casControlNames; - int numTotalControls = static_cast(controls.rows()) + + int numTotalControls = static_cast(controls.rows()) + static_cast(input_controls.rows()); casadi::DM casControls(numTotalControls, controls.columns()); - + int ic = 0; int iic = 0; std::sort(inputControlIndexes.begin(), inputControlIndexes.end()); int numInputControls = static_cast(inputControlIndexes.size()); for (int i = 0; i < numTotalControls; ++i) { if (iic < numInputControls && inputControlIndexes[iic] == i) { - casControls(i, casadi::Slice()) = + casControls(i, casadi::Slice()) = input_controls(iic, casadi::Slice()); casControlNames.push_back(inputControlNames[iic]); ++iic; @@ -108,24 +110,78 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, casVars[Var::controls] = casControls; casVars[Var::multipliers] = - convertToCasADiDMTranspose(mocoIt.getMultipliersTrajectory()); - if (!mocoIt.getSlackNames().empty()) { - casVars[Var::slacks] = - convertToCasADiDMTranspose(mocoIt.getSlacksTrajectory()); - } - if (!mocoIt.getDerivativeNames().empty()) { + convertToCasADiDMTranspose(mocoTraj.getMultipliersTrajectory()); + + if (!mocoTraj.getDerivativeNames().empty()) { casVars[Var::derivatives] = - convertToCasADiDMTranspose(mocoIt.getDerivativesTrajectory()); + convertToCasADiDMTranspose(mocoTraj.getDerivativesTrajectory()); } casVars[Var::parameters] = - convertToCasADiDMTranspose(mocoIt.getParameters()); - casIt.times = convertToCasADiDMTranspose(mocoIt.getTime()); - casIt.state_names = mocoIt.getStateNames(); + convertToCasADiDMTranspose(mocoTraj.getParameters()); + if (appendProjectionStates) { + casVars[Var::projection_states] = convertToCasADiDMTranspose( + mocoTraj.getMultibodyStatesTrajectory()); + } + casIt.times = convertToCasADiDMTranspose(mocoTraj.getTime()); + casIt.state_names = mocoTraj.getStateNames(); casIt.control_names = casControlNames; - casIt.multiplier_names = mocoIt.getMultiplierNames(); - casIt.slack_names = mocoIt.getSlackNames(); - casIt.derivative_names = mocoIt.getDerivativeNames(); - casIt.parameter_names = mocoIt.getParameterNames(); + casIt.multiplier_names = mocoTraj.getMultiplierNames(); + casIt.derivative_names = mocoTraj.getDerivativeNames(); + casIt.parameter_names = mocoTraj.getParameterNames(); + + // Projection state variables. + // --------------------------- + // Extra variables needed when using the projection method for enforcing + // kinematic constraints from Bordalba et al. (2023). + if (appendProjectionStates) { + auto mbStateNames = mocoTraj.getMultibodyStateNames(); + for (auto name : mbStateNames) { + auto valuepos = name.find("/value"); + if (valuepos != std::string::npos) { + name.replace(valuepos, 6, "/value/projection"); + casIt.projection_state_names.push_back(name); + } + auto speedpos = name.find("/speed"); + if (speedpos != std::string::npos) { + name.replace(speedpos, 6, "/speed/projection"); + casIt.projection_state_names.push_back(name); + } + } + } + // Slack variables. + // ---------------- + // Check that the trajectory has the expected slack names from the + // CasOCProblem. + bool matchedExpectedSlackNames = + mocoTraj.getSlackNames().size() == expectedSlackNames.size(); + if (matchedExpectedSlackNames) { + for (const auto& expectedName : expectedSlackNames) { + if (std::find(mocoTraj.getSlackNames().begin(), + mocoTraj.getSlackNames().end(), expectedName) == + mocoTraj.getSlackNames().end()) { + matchedExpectedSlackNames = false; + break; + } + } + } + + // If the guess matches the expected slack names, use the slack values from + // the guess. If not, create a vector of zeros to use as the initial guess + // for the slack variables. + if (matchedExpectedSlackNames) { + casIt.slack_names = mocoTraj.getSlackNames(); + if (!mocoTraj.getSlackNames().empty()) { + casVars[Var::slacks] = + convertToCasADiDMTranspose(mocoTraj.getSlacksTrajectory()); + } + } else { + casIt.slack_names = expectedSlackNames; + if (!expectedSlackNames.empty()) { + casVars[Var::slacks] = casadi::DM::zeros( + (int)expectedSlackNames.size(), mocoTraj.getNumTimes()); + } + } + return casIt; } @@ -155,7 +211,7 @@ inline SimTK::Matrix convertToSimTKMatrix(const casadi::DM& casMatrix) { } template -TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, +TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, std::vector inputControlIndexes = {}) { SimTK::Matrix simtkStates; const auto& casVars = casIt.variables; @@ -172,7 +228,7 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, std::vector controlNames; std::vector inputControlNames; if (numTotalControls) { - SimTK::Matrix allControls = + SimTK::Matrix allControls = convertToSimTKMatrix(casVars.at(Var::controls)); simtkControls.resize(allControls.nrow(), numControls); simtkInputControls.resize(allControls.nrow(), numInputControls); @@ -217,14 +273,14 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, } SimTK::Vector simtkTimes = convertToSimTKVector(casIt.times); - TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, - inputControlNames, casIt.multiplier_names, derivativeNames, - casIt.parameter_names, simtkStates, simtkControls, - simtkInputControls, simtkMultipliers, simtkDerivatives, + TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, + inputControlNames, casIt.multiplier_names, derivativeNames, + casIt.parameter_names, simtkStates, simtkControls, + simtkInputControls, simtkMultipliers, simtkDerivatives, simtkParameters); // Append slack variables. MocoTrajectory requires the slack variables to be - // the same length as its time vector, but it will not be if the + // the same length as its time vector, but it might not be if the // CasOC::Iterate was generated from a CasOC::Transcription object. // Therefore, slack variables are interpolated as necessary. if (!casIt.slack_names.empty()) { @@ -252,7 +308,8 @@ class MocoCasOCProblem : public CasOC::Problem { MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, const MocoProblemRep& mocoProblemRep, std::unique_ptr> jar, - std::string dynamicsMode); + std::string dynamicsMode, + std::string kinematicConstraintMethod); int getJarSize() const { return (int)m_jar->size(); } @@ -278,9 +335,10 @@ class MocoCasOCProblem : public CasOC::Problem { modelDisabledConstraints.realizeAcceleration( simtkStateDisabledConstraints); - // Compute kinematic constraint errors if they exist. + // Compute kinematic constraint errors. if (getNumMultipliers() && calcKCErrors) { - calcKinematicConstraintErrors(modelBase, simtkStateBase, + calcKinematicConstraintErrors( + modelBase, simtkStateBase, simtkStateDisabledConstraints, output.kinematic_constraint_errors); } @@ -309,7 +367,7 @@ class MocoCasOCProblem : public CasOC::Problem { const auto& modelBase = mocoProblemRep->getModelBase(); auto& simtkStateBase = mocoProblemRep->updStateBase(); - // Model with disabled constriants and its associated state. These are + // Model with disabled constraints and its associated state. These are // used to compute the accelerations. const auto& modelDisabledConstraints = mocoProblemRep->getModelDisabledConstraints(); @@ -323,13 +381,10 @@ class MocoCasOCProblem : public CasOC::Problem { modelDisabledConstraints.realizeAcceleration( simtkStateDisabledConstraints); - // Compute kinematic constraint errors if they exist. - // TODO: Do not enforce kinematic constraints if prescribedKinematics, - // but must make sure the prescribedKinematics already obey the - // constraints. This is simple at the q and u level (using assemble()), - // but what do we do for the acceleration level? + // Compute kinematic constraint errors. if (getNumMultipliers() && calcKCErrors) { - calcKinematicConstraintErrors(modelBase, simtkStateBase, + calcKinematicConstraintErrors( + modelBase, simtkStateBase, simtkStateDisabledConstraints, output.kinematic_constraint_errors); } @@ -387,6 +442,50 @@ class MocoCasOCProblem : public CasOC::Problem { m_jar->leave(std::move(mocoProblemRep)); } + void calcStateProjection(const double& time, + const casadi::DM& multibody_states, const casadi::DM& slacks, + const casadi::DM& parameters, + casadi::DM& projection) const override { + if (isPrescribedKinematics()) return; + auto mocoProblemRep = m_jar->take(); + + const auto& modelBase = mocoProblemRep->getModelBase(); + auto& simtkStateBase = mocoProblemRep->updStateBase(); + + // Update the model and state. + applyParametersToModelProperties(parameters, *mocoProblemRep); + convertStatesToSimTKState( + SimTK::Stage::Velocity, time, multibody_states, + modelBase, simtkStateBase, false); + modelBase.realizeVelocity(simtkStateBase); + + // Compute the state projection vector based on the method by Bordalba + // et al. (2023). Our implementation looks slightly different from the + // projection constraints in the manuscript since we compute the + // projections for the coordinate values and coordinate speeds + // separately based on how Simbody's assembler handles coordinate + // projections for kinematic constraints. + const SimTK::SimbodyMatterSubsystem& matterBase = + modelBase.getMatterSubsystem(); + + // Holonomic constraint errors. + const SimTK::Vector mu_p( + getNumHolonomicConstraintEquations(), slacks.ptr(), true); + SimTK::Vector proj_p(getNumCoordinates(), projection.ptr(), true); + matterBase.multiplyByPqTranspose(simtkStateBase, mu_p, proj_p); + + // Derivative of holonomic constraint errors and non-holonomic + // constraint errors. + const SimTK::Vector mu_v( + getNumHolonomicConstraintEquations() + + getNumNonHolonomicConstraintEquations(), + slacks.ptr() + getNumHolonomicConstraintEquations(), true); + SimTK::Vector proj_v(getNumSpeeds(), projection.ptr() + + getNumCoordinates(), true); + matterBase.multiplyByPVTranspose(simtkStateBase, mu_v, proj_v); + + m_jar->leave(std::move(mocoProblemRep)); + } void calcCostIntegrand(int index, const ContinuousInput& input, double& integrand) const override { auto mocoProblemRep = m_jar->take(); @@ -611,8 +710,8 @@ class MocoCasOCProblem : public CasOC::Problem { for (int ic = 0; ic < getNumControls(); ++ic) { simtkControls[ic] = *(controls.ptr() + ic); } - // Updating the Inputs to InputControllers via the - // ControlDistributor does not mark the model controls cache as + // Updating the Inputs to InputControllers via the + // ControlDistributor does not mark the model controls cache as // invalid, so we must do it manually here. model.markControlsAsInvalid(simtkState); } @@ -714,7 +813,8 @@ class MocoCasOCProblem : public CasOC::Problem { m_constraintMobilityForces, m_constraintBodyForces); } - void calcKinematicConstraintErrors(const Model& modelBase, + void calcKinematicConstraintErrors( + const Model& modelBase, const SimTK::State& stateBase, const SimTK::State& simtkStateDisabledConstraints, casadi::DM& kinematic_constraint_errors) const { @@ -724,23 +824,14 @@ class MocoCasOCProblem : public CasOC::Problem { // constraints would be redundant, and we need not enforce them. if (isPrescribedKinematics()) return; - // The total number of scalar holonomic, non-holonomic, and acceleration - // constraint equations enabled in the model. This does not count - // equations for derivatives of holonomic and non-holonomic constraints. - const int total_mp = getNumHolonomicConstraintEquations(); - const int total_mv = getNumNonHolonomicConstraintEquations(); - const int total_ma = getNumAccelerationConstraintEquations(); - - // Position-level errors. - const auto& qerr = stateBase.getQErr(); - - if (getEnforceConstraintDerivatives() || total_ma) { - // Calculate udoterr. We cannot use State::getUDotErr() - // because that uses Simbody's multipliers and UDot, - // whereas we have our own multipliers and UDot. Here, we use - // the udot computed from the model with disabled constraints - // since we cannot use (nor do we have available) udot computed - // from the original model. + // Calculate udoterr. We cannot use State::getUDotErr() + // because that uses Simbody's multipliers and UDot, + // whereas we have our own multipliers and UDot. Here, we use + // the udot computed from the model with disabled constraints + // since we cannot use (nor do we have available) udot computed + // from the original model. + if (getEnforceConstraintDerivatives() || + getNumAccelerationConstraintEquations()) { const auto& matter = modelBase.getMatterSubsystem(); matter.calcConstraintAccelerationErrors(stateBase, simtkStateDisabledConstraints.getUDot(), m_pvaerr); @@ -748,40 +839,23 @@ class MocoCasOCProblem : public CasOC::Problem { m_pvaerr = SimTK::NaN; } + // Position-level errors. + const auto& qerr = stateBase.getQErr(); + // Velocity-level errors. const auto& uerr = stateBase.getUErr(); - int uerrOffset; - int uerrSize; + // Acceleration-level errors. const auto& udoterr = m_pvaerr; - int udoterrOffset; - int udoterrSize; - // TODO These offsets and sizes could be computed once. - if (getEnforceConstraintDerivatives()) { - // Velocity-level errors. - uerrOffset = 0; - uerrSize = uerr.size(); - // Acceleration-level errors. - udoterrOffset = 0; - udoterrSize = udoterr.size(); - } else { - // Velocity-level errors. Skip derivatives of position-level - // constraint equations. - uerrOffset = total_mp; - uerrSize = total_mv; - // Acceleration-level errors. Skip derivatives of velocity- - // and position-level constraint equations. - udoterrOffset = total_mp + total_mv; - udoterrSize = total_ma; - } // This way of copying the data avoids a threadsafety issue in // CasADi related to cached Sparsity objects. std::copy_n(qerr.getContiguousScalarData(), qerr.size(), kinematic_constraint_errors.ptr()); - std::copy_n(uerr.getContiguousScalarData() + uerrOffset, uerrSize, + std::copy_n(uerr.getContiguousScalarData() + m_uerrOffset, m_uerrSize, kinematic_constraint_errors.ptr() + qerr.size()); - std::copy_n(udoterr.getContiguousScalarData() + udoterrOffset, - udoterrSize, - kinematic_constraint_errors.ptr() + qerr.size() + uerrSize); + std::copy_n(udoterr.getContiguousScalarData() + m_udoterrOffset, + m_udoterrSize, + kinematic_constraint_errors.ptr() + qerr.size() + m_uerrSize); + } void copyImplicitResidualsToOutput(const MocoProblemRep& mocoProblemRep, @@ -812,6 +886,12 @@ class MocoCasOCProblem : public CasOC::Problem { // the acceleration-level holonomic, non-holonomic constraint errors and the // acceleration-only constraint errors. static thread_local SimTK::Vector m_pvaerr; + // These offsets are necessary when not enforcing the derivatives of the + // kinematic constraint equations. + int m_uerrOffset; + int m_uerrSize; + int m_udoterrOffset; + int m_udoterrSize; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoConstraintInfo.cpp b/OpenSim/Moco/MocoConstraintInfo.cpp index def5a55aaa..a304a7f6c7 100644 --- a/OpenSim/Moco/MocoConstraintInfo.cpp +++ b/OpenSim/Moco/MocoConstraintInfo.cpp @@ -49,7 +49,7 @@ void MocoConstraintInfo::printDescription() const { boundsStr.push_back(ss.str()); } str += fmt::format(". bounds: {}", fmt::join(boundsStr, ", ")); - log_cout(str); + log_info(str); } void MocoConstraintInfo::constructProperties() { diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.cpp b/OpenSim/Moco/MocoDirectCollocationSolver.cpp index b4f4acb34c..5388f91243 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.cpp +++ b/OpenSim/Moco/MocoDirectCollocationSolver.cpp @@ -25,7 +25,7 @@ void MocoDirectCollocationSolver::constructProperties() { constructProperty_mesh(); constructProperty_verbosity(2); constructProperty_transcription_scheme("hermite-simpson"); - constructProperty_interpolate_control_midpoints(true); + constructProperty_interpolate_control_mesh_interior_points(true); constructProperty_enforce_constraint_derivatives(true); constructProperty_multibody_dynamics_mode("explicit"); constructProperty_optim_solver("ipopt"); @@ -40,6 +40,7 @@ void MocoDirectCollocationSolver::constructProperties() { constructProperty_implicit_auxiliary_derivative_bounds({-1000, 1000}); constructProperty_minimize_lagrange_multipliers(false); constructProperty_lagrange_multiplier_weight(1.0); + constructProperty_kinematic_constraint_method("Posa2016"); } void MocoDirectCollocationSolver::setMesh(const std::vector& mesh) { @@ -86,3 +87,42 @@ void MocoDirectCollocationSolver::checkConstraintJacobianRank( log_warn(dashes); } } + +void MocoDirectCollocationSolver::checkSlackVariables( + const MocoSolution& solution) const { + const auto& slacks = solution.getSlacksTrajectory(); + const auto& slackNames = solution.getSlackNames(); + const SimTK::Real threshold = 1e-3; + bool largeSlackDetected = false; + + std::vector slackWarnings; + for (int islack = 0; islack < slacks.ncol(); ++islack) { + if (SimTK::max(SimTK::abs(slacks.col(islack))) > threshold) { + largeSlackDetected = true; + std::stringstream ss; + ss << "Slack variable '" << slackNames[islack] << "' has a maximum " + << "value of " << SimTK::max(SimTK::abs(slacks.col(islack))) + << "."; + slackWarnings.push_back(ss.str()); + } + } + + if (largeSlackDetected) { + const std::string dashes(50, '-'); + log_warn(dashes); + log_warn("Large slack variables detected."); + log_warn(dashes); + for (const auto& warning : slackWarnings) { + log_warn(warning); + } + log_warn(""); + log_warn("Slack variables with values larger than ~{} might", threshold); + log_warn("indicate that the problem is struggling to enforce"); + log_warn("kinematic constraints in the problem. Since slack variables"); + log_warn("interact with defect constraints in the direct collocation "); + log_warn("formulation, large slack values may affect the quality "); + log_warn("of the solution. Consider refining the mesh or adjusting "); + log_warn("the constraint tolerance in the MocoProblem."); + log_warn(dashes); + } +} diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.h b/OpenSim/Moco/MocoDirectCollocationSolver.h index e09f99e391..d671d17b15 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.h +++ b/OpenSim/Moco/MocoDirectCollocationSolver.h @@ -99,13 +99,19 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "0 for silent. 1 for only Moco's own output. " "2 for output from CasADi and the underlying solver (default: 2)."); OpenSim_DECLARE_PROPERTY(transcription_scheme, std::string, - "'trapezoidal' for trapezoidal transcription, or 'hermite-simpson' " - "(default) for separated Hermite-Simpson transcription."); - OpenSim_DECLARE_PROPERTY(interpolate_control_midpoints, bool, - "If the transcription scheme is set to 'hermite-simpson', then " - "enable this property to constrain the control values at mesh " - "interval midpoints to be linearly interpolated from the control " - "values at the mesh interval endpoints. Default: true."); + "'trapezoidal' for trapezoidal transcription, 'hermite-simpson' " + "(default) for separated Hermite-Simpson transcription, " + "'legendre-gauss-#' for Legendre-Gauss transcription (where # is " + "the number of collocation points per mesh interval between 1 and " + "9), and 'legendre-gauss-radau-#' for Legendre-Gauss-Radau " + "transcription."); + OpenSim_DECLARE_PROPERTY(interpolate_control_mesh_interior_points, bool, + "If the transcription scheme is set to 'hermite-simpson' or one of " + "the pseudospectral schemes (e.g., 'legendre-gauss-3', " + "'legendre-gauss-radau-3') then enable this property to constrain " + "the control values at mesh interior points to be linearly " + "interpolated from the control values at the mesh interval " + "endpoints. Default: true."); OpenSim_DECLARE_PROPERTY(multibody_dynamics_mode, std::string, "Multibody dynamics are expressed as 'explicit' (default) or " "'implicit' differential equations."); @@ -147,6 +153,7 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "enforced, set the bounds on the slack variables performing the " "velocity correction to project the model coordinates back onto " "the constraint manifold. Default: [-0.1, 0.1]"); + OpenSim_DECLARE_PROPERTY(implicit_multibody_acceleration_bounds, MocoBounds, "Bounds on acceleration variables in implicit dynamics mode. " "Default: [-1000, 1000]"); @@ -154,6 +161,12 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "Bounds on derivative variables for components with auxiliary " "dynamics in implicit form. Default: [-1000, 1000]"); + OpenSim_DECLARE_PROPERTY(kinematic_constraint_method, std::string, + "The method used to enforce kinematic constraints in the direct " + "collocation problem. 'Posa2016' for the method by Posa et al. " + "2016 (default) or 'Bordalba2023' for the method by Bordalba et " + "al. 2023 (only valid with CasADi)."); + MocoDirectCollocationSolver() { constructProperties(); } /** %Set the mesh to a user-defined list of mesh points to sample. This @@ -170,8 +183,9 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "Takes precedence over uniform mesh with num_mesh_intervals."); void constructProperties(); - // Helper function for post-processing the solution. + // Helper functions for post-processing the solution. void checkConstraintJacobianRank(const MocoSolution& mocoSolution) const; + void checkSlackVariables(const MocoSolution& mocoSolution) const; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp index a74ba15a45..0756adba36 100644 --- a/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp @@ -136,10 +136,10 @@ void MocoAccelerationTrackingGoal::calcIntegrandImpl( } void MocoAccelerationTrackingGoal::printDescriptionImpl() const { - log_cout(" acceleration reference file: {}", + log_info(" acceleration reference file: {}", get_acceleration_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", i, m_frame_paths[i], + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_acceleration_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp index f09224c8f0..2fe8728b00 100644 --- a/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp @@ -163,10 +163,10 @@ void MocoAngularVelocityTrackingGoal::calcIntegrandImpl( } void MocoAngularVelocityTrackingGoal::printDescriptionImpl() const { - log_cout(" angular velocity reference file: {}", + log_info(" angular velocity reference file: {}", get_angular_velocity_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", i, m_frame_paths[i], + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_angular_velocity_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp index 6201ac9d47..abb6260c8a 100644 --- a/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp @@ -340,15 +340,15 @@ void MocoContactImpulseTrackingGoal::calcIntegrandImpl( } void MocoContactImpulseTrackingGoal::printDescriptionImpl() const { - log_cout(" impulse axis: {}", get_impulse_axis()); + log_info(" impulse axis: {}", get_impulse_axis()); for (int ig = 0; ig < getProperty_contact_groups().size(); ++ig) { const auto& group = get_contact_groups(ig); - log_cout(" group {}: ExternalForce: {}", + log_info(" group {}: ExternalForce: {}", ig, group.get_external_force_name()); - log_cout(" forces:"); + log_info(" forces:"); for (int ic = 0; ic < group.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", group.get_contact_force_paths(ic)); + log_info(" {}", group.get_contact_force_paths(ic)); } } } \ No newline at end of file diff --git a/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp index 17fc6c23d3..1aeb951879 100644 --- a/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp @@ -374,18 +374,18 @@ void MocoContactTrackingGoal::calcIntegrandImpl( } void MocoContactTrackingGoal::printDescriptionImpl() const { - log_cout(" projection type: {}", get_projection()); + log_info(" projection type: {}", get_projection()); if (m_projectionType != ProjectionType::None) { - log_cout(" projection vector: {}", get_projection_vector()); + log_info(" projection vector: {}", get_projection_vector()); } for (int ig = 0; ig < getProperty_contact_groups().size(); ++ig) { const auto& group = get_contact_groups(ig); - log_cout(" group {}: ExternalForce: {}", + log_info(" group {}: ExternalForce: {}", ig, group.get_external_force_name()); - log_cout(" forces:"); + log_info(" forces:"); for (int ic = 0; ic < group.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", group.get_contact_force_paths(ic)); + log_info(" {}", group.get_contact_force_paths(ic)); } } } diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp index c01ec362a0..334ef0422f 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp @@ -194,11 +194,11 @@ void MocoControlGoal::calcGoalImpl( void MocoControlGoal::printDescriptionImpl() const { for (int i = 0; i < (int) m_controlNames.size(); i++) { - log_cout(" control: {}, weight: {}", m_controlNames[i], + log_info(" control: {}, weight: {}", m_controlNames[i], m_weights[i]); } for (int i = 0; i < (int) m_inputControlNames.size(); i++) { - log_cout(" Input control: {}, weight: {}", + log_info(" Input control: {}, weight: {}", m_inputControlNames[i], m_inputControlWeights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp index ff10037b79..56a4098aee 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp @@ -259,7 +259,7 @@ void MocoControlTrackingGoal::calcIntegrandImpl( void MocoControlTrackingGoal::printDescriptionImpl() const { for (int i = 0; i < (int)m_control_names.size(); i++) { std::string type = m_isInputControl[i] ? "Input control" : "control"; - log_cout(" {}: {}, reference label: {}, weight: {}", + log_info(" {}: {}, reference label: {}, weight: {}", type, m_control_names[i], m_ref_labels[i], m_control_weights[i]); } diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp new file mode 100644 index 0000000000..0e316ecf3b --- /dev/null +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -0,0 +1,124 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: MocoExpressionBasedParameterGoal.cpp * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoExpressionBasedParameterGoal.h" + +#include +#include +#include +#include + +using namespace OpenSim; + +void MocoExpressionBasedParameterGoal::constructProperties() { + constructProperty_expression(""); + constructProperty_parameters(); + constructProperty_variables(); +} + +void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) + const { + OPENSIM_THROW_IF_FRMOBJ(get_expression().empty(), Exception, + "The expression has not been set. Use setExpression().") + m_program = Lepton::Parser::parse(get_expression()).optimize() + .createProgram(); + setRequirements(0, 1, SimTK::Stage::Instance); + + for (int i = 0; i < getProperty_parameters().size(); i++) { + // only taking the first one since they should all be the same value + std::string componentPath = get_parameters(i).getComponentPaths()[0]; + const auto& component = model.getComponent(componentPath); + const auto* ap = &component.getPropertyByName( + get_parameters(i).getPropertyName()); + m_property_refs.emplace_back(ap); + + // get the type and element of the property + if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_double); + } else { + if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_Vec3); + m_indices.emplace_back(get_parameters(i).getPropertyElement()); + } + else if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_Vec6); + m_indices.emplace_back(get_parameters(i).getPropertyElement()); + } + else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified model property not supported."); + } + } + } + + // test to make sure all variables are there + try { + std::map parameterVars; + for (int i = 0; i < getProperty_variables().size(); ++i) { + parameterVars[get_variables(i)] = getPropertyValue(i); + } + m_program.evaluate(parameterVars); + } catch (Lepton::Exception& ex) { + const std::string msg = ex.what(); + if (msg.compare(0, 30, "No value specified for variable")) { + std::string undefinedVar = msg.substr(32, msg.size() - 32); + OPENSIM_THROW_FRMOBJ(Exception, + fmt::format("Parameter variable '{}' is not defined. Use " + "addParameter() to explicitly define this variable. Or, " + "remove it from the expression.", undefinedVar)); + } else { + OPENSIM_THROW_FRMOBJ(Exception, "Lepton parsing error: {}", msg); + } + } +} + +double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { + OPENSIM_ASSERT_FRMOBJ(m_property_refs.size() > i); + const auto& propRef = m_property_refs[i]; + if (m_data_types[i] == Type_double) { + return static_cast*>(propRef.get())->getValue(); + } + int elt = m_indices[i]; + if (m_data_types[i] == Type_Vec3) { + return static_cast*>(propRef.get()) + ->getValue()[elt]; + } + if (m_data_types[i] == Type_Vec6) { + return static_cast*>(propRef.get()) + ->getValue()[elt]; + } + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Property at index {} is not of" + " a recognized type.")); +} + +void MocoExpressionBasedParameterGoal::calcGoalImpl( + const GoalInput& input, SimTK::Vector& values) const { + std::map parameterVars; + for (int i = 0; i < getProperty_variables().size(); ++i) { + parameterVars[get_variables(i)] = getPropertyValue(i); + } + values[0] = m_program.evaluate(parameterVars); +} + +void MocoExpressionBasedParameterGoal::printDescriptionImpl() const { + log_info(" expression: {}", get_expression()); + for (int i = 0; i < getProperty_parameters().size(); ++i) { + log_cout(" variable {}: {}", get_variables(i), + get_parameters(i).getName()); + } +} diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h new file mode 100644 index 0000000000..2de96dd42d --- /dev/null +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -0,0 +1,133 @@ +#ifndef OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H +#define OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H +/* -------------------------------------------------------------------------- * + * OpenSim: MocoExpressionBasedParameterGoal.h * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoGoal.h" +#include "OpenSim/Moco/MocoParameter.h" +#include +#include + +namespace OpenSim { +class Model; + +/** Minimize or constrain an arithmetic expression of parameters. + +This goal supports both "cost" and "endpoint constraint" modes and can be +defined using any number of MocoParameters. The expression string should match +the Lepton (lightweight expression parser) format. + +# Creating Expressions + +Expressions can be any string that represents a mathematical expression, e.g., +"x*sqrt(y-8)". Expressions can contain variables, constants, operations, +parentheses, commas, spaces, and scientific "e" notation. The full list of +operations is: sqrt, exp, log, sin, cos, sec, csc, tan, cot, asin, acos, atan, +sinh, cosh, tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, ++, -, *, /, and ^. + +# Examples + +@code +auto* spring1_parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); +auto* spring2_parameter = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); +auto* spring_goal = mp.addGoal(); +double STIFFNESS = 100.0; +// minimum is when p + q = STIFFNESS +spring_goal->setExpression(fmt::format("square(p+q-{})", STIFFNESS)); +spring_goal->addParameter(*spring1_parameter, "p"); +spring_goal->addParameter(*spring2_parameter, "q"); +@endcode + +@ingroup mocogoal */ +class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { + OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal); + +public: + MocoExpressionBasedParameterGoal() { constructProperties(); } + MocoExpressionBasedParameterGoal(std::string name) + : MocoGoal(std::move(name)) { + constructProperties(); + } + MocoExpressionBasedParameterGoal(std::string name, double weight) + : MocoGoal(std::move(name), weight) { + constructProperties(); + } + MocoExpressionBasedParameterGoal(std::string name, double weight, + std::string expression) : MocoGoal(std::move(name), weight) { + constructProperties(); + set_expression(std::move(expression)); + } + + /** Set the arithmetic expression to minimize or constrain. Variable + names should match the names set with addParameter(). See "Creating + Expressions" in the class documentation above for an explanation of how to + create expressions. */ + void setExpression(std::string expression) { + set_expression(std::move(expression)); + } + + /** Add parameters with variable names that match the variables in the + expression string. All variables in the expression must have a corresponding + parameter, but parameters with variables that are not in the expression are + ignored. */ + void addParameter(const MocoParameter& parameter, std::string variable) { + append_parameters(parameter); + append_variables(std::move(variable)); + } + +protected: + void initializeOnModelImpl(const Model& model) const override; + void calcGoalImpl( + const GoalInput& input, SimTK::Vector& cost) const override; + bool getSupportsEndpointConstraintImpl() const override { return true; } + void printDescriptionImpl() const override; + +private: + void constructProperties(); + + /** Get the value of the property from its index in the property_refs vector. + This will use m_data_types to get the type, and if it is a Vec type, it uses + m_indices to get the element to return, both at the same index i. */ + double getPropertyValue(int i) const; + + OpenSim_DECLARE_PROPERTY(expression, std::string, + "The expression string defining this cost or endpoint constraint."); + OpenSim_DECLARE_LIST_PROPERTY(parameters, MocoParameter, + "Parameters included in the expression."); + OpenSim_DECLARE_LIST_PROPERTY(variables, std::string, + "Variables names corresponding to parameters in the expression."); + + mutable Lepton::ExpressionProgram m_program; + // stores references to one property per parameter + mutable std::vector> m_property_refs; + enum DataType { + Type_double, + Type_Vec3, + Type_Vec6 + }; + mutable std::vector m_data_types; + mutable std::vector m_indices; + +}; + +} // namespace OpenSim + +#endif // OPENSIM_MOCOPARAMETEREXPRESSIONGOAL_H diff --git a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp index 32af44d5c7..b96932c8ed 100644 --- a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp @@ -29,11 +29,31 @@ void MocoGeneralizedForceTrackingGoal::constructProperties() { constructProperty_reference(TableProcessor()); constructProperty_force_paths(); constructProperty_generalized_force_weights(MocoWeightSet()); + constructProperty_generalized_force_weights_pattern(MocoWeightSet()); constructProperty_allow_unused_references(false); constructProperty_normalize_tracking_error(false); constructProperty_ignore_constrained_coordinates(true); } +void MocoGeneralizedForceTrackingGoal::setWeightForGeneralizedForce( + const std::string& name, double weight) { + if (get_generalized_force_weights().contains(name)) { + upd_generalized_force_weights().get(name).setWeight(weight); + } else { + upd_generalized_force_weights().cloneAndAppend({name, weight}); + } +} + +void MocoGeneralizedForceTrackingGoal::setWeightForGeneralizedForcePattern( + const std::string& pattern, double weight) { + if (get_generalized_force_weights_pattern().contains(pattern)) { + upd_generalized_force_weights_pattern().get(pattern).setWeight(weight); + } else { + upd_generalized_force_weights_pattern().cloneAndAppend( + {pattern, weight}); + } +} + void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( const Model& model) const { @@ -47,7 +67,7 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( // Create a map between coordinate paths and their indices in the system. SimTK::State state = model.getWorkingState(); const auto& coordinates = model.getCoordinatesInMultibodyTreeOrder(); - std::unordered_map allCoordinateIndices; + std::unordered_map allGeneralizedForceIndexes; for (int i = 0; i < static_cast(coordinates.size()); ++i) { if (get_ignore_constrained_coordinates() && coordinates[i]->isConstrained(state)) { @@ -72,7 +92,7 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( coordinates[i]->getName()); } - allCoordinateIndices[label] = i; + allGeneralizedForceIndexes[label] = i; } // Get the system indexes for specified forces. @@ -100,27 +120,40 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( const SimTK::Force::Gravity& gravity = model.getGravityForce(); m_forceIndexes.push_back(gravity.getForceIndex()); + // Set the regex pattern weights first. + std::map weightsFromPatterns; + for (int i = 0; i < get_generalized_force_weights_pattern().getSize(); ++i) { + const auto& mocoWeight = get_generalized_force_weights_pattern().get(i); + const auto& pattern = mocoWeight.getName(); + const auto regex = std::regex(pattern); + for (const auto& kv : allGeneralizedForceIndexes) { + if (std::regex_match(kv.first, regex)) { + weightsFromPatterns[kv.first] = mocoWeight.getWeight(); + } + } + } + // Validate the coordinate weights. for (int i = 0; i < get_generalized_force_weights().getSize(); ++i) { const auto& weightName = get_generalized_force_weights().get(i).getName(); - if (allCoordinateIndices.count(weightName) == 0) { + if (allGeneralizedForceIndexes.count(weightName) == 0) { OPENSIM_THROW_FRMOBJ(Exception, "Weight provided with name '{}' but this is " - "not a recognized coordinate or it is a constrained " - "coordinate and set to be ignored.", + "not a recognized generalized force or it is associated " + "with a constrained coordinate and set to be ignored.", weightName); } } for (int iref = 0; iref < allSplines.getSize(); ++iref) { const auto& refName = allSplines[iref].getName(); - if (allCoordinateIndices.count(refName) == 0) { + if (allGeneralizedForceIndexes.count(refName) == 0) { if (get_allow_unused_references()) { continue; } OPENSIM_THROW_FRMOBJ(Exception, "Reference provided with name '{}' but this is " - "not a recognized coordinate or it is a constrained " - "coordinate and set to be ignored.", + "not a recognized generalized force or it is associated " + "with a constrained coordinate and set to be ignored.", refName); } @@ -132,12 +165,21 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( "Expected coordinate weights to be non-negative, but " "received a negative weight for coordinate '{}'.", refName); - if (refWeight < SimTK::SignificantReal) { continue; } + } else if (weightsFromPatterns.count(refName)) { + refWeight = weightsFromPatterns[refName]; + } + + if (refWeight > SimTK::SignificantReal) { + m_generalizedForceIndexes.push_back( + allGeneralizedForceIndexes.at(refName)); + m_generalizedForceWeights.push_back(refWeight); + m_generalizedForceNames.push_back(refName); + m_refsplines.cloneAndAppend(allSplines[iref]); + } else { + log_info("MocoGeneralizedForceTrackingGoal: Generalized force '{}' " + "has weight 0 (or very close to 0) and will be ignored.", + refName); } - m_coordinateIndexes.push_back(allCoordinateIndices.at(refName)); - m_generalizedForceWeights.push_back(refWeight); - m_generalizedForceNames.push_back(refName); - m_refsplines.cloneAndAppend(allSplines[iref]); // Compute normalization factors. if (get_normalize_tracking_error()) { @@ -185,7 +227,8 @@ void MocoGeneralizedForceTrackingGoal::calcIntegrandImpl( SimTK::Vector timeVec(1, time); integrand = 0; for (int iref = 0; iref < m_refsplines.getSize(); ++iref) { - const auto& modelValue = generalizedForces[m_coordinateIndexes[iref]]; + const auto& modelValue = + generalizedForces[m_generalizedForceIndexes[iref]]; const auto& refValue = m_refsplines[iref].calcValue(timeVec); // Compute the tracking error. @@ -198,7 +241,7 @@ void MocoGeneralizedForceTrackingGoal::calcIntegrandImpl( void MocoGeneralizedForceTrackingGoal::printDescriptionImpl() const { for (int i = 0; i < static_cast(m_generalizedForceNames.size()); i++) { - log_cout(" generalized force: {}, weight: {}", + log_info(" generalized force: {}, weight: {}", m_generalizedForceNames[i], m_generalizedForceWeights[i]); } diff --git a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h index ceb2e05f9a..05677c8d90 100644 --- a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h @@ -93,13 +93,15 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); /// then the provided weight replaces the previous weight. Weight names /// should match the column labels in the reference table (e.g., /// `ankle_angle_r_moment`, `pelvis_tx_force`, etc.). - void setWeightForGeneralizedForce(const std::string& name, double weight) { - if (get_generalized_force_weights().contains(name)) { - upd_generalized_force_weights().get(name).setWeight(weight); - } else { - upd_generalized_force_weights().cloneAndAppend({name, weight}); - } - } + void setWeightForGeneralizedForce(const std::string& name, double weight); + + /// Set the tracking weight for all generalized forces whose names match the + /// regular expression pattern. Multiple pairs of patterns and weights can + /// be provided by calling this function multiple times. If a generalized + /// force matches multiple patterns, the weight associated with the last + /// pattern is used. + void setWeightForGeneralizedForcePattern(const std::string& pattern, + double weight); /// Set the MocoWeightSet to weight the generalized coordinate forces in /// the cost. Replaces the weight set if it already exists. @@ -122,9 +124,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); /// @copydoc setNormalizeTrackingError(bool tf) bool getNormalizeTrackingError() { return get_normalize_tracking_error(); } - /// Whether or not to ignore coordinates that are locked, prescribed, or - /// coupled to other coordinates. This is based on the value returned from - /// `Coordinate::isConstrained()` (default: true). + /// Whether or not to ignore generalized forces for coordinates that are + /// locked, prescribed, or coupled to other coordinates. This is based on + /// the value returned from `Coordinate::isConstrained()` (default: true). void setIgnoreConstrainedCoordinates(bool tf) { set_ignore_constrained_coordinates(tf); } @@ -156,6 +158,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); OpenSim_DECLARE_PROPERTY(generalized_force_weights, MocoWeightSet, "Set of weight objects to weight the tracking of individual " "generalized coordinate forces in the cost."); + OpenSim_DECLARE_PROPERTY(generalized_force_weights_pattern, MocoWeightSet, + "Set weights for all generalized forces matching a regular " + "expression."); OpenSim_DECLARE_PROPERTY(allow_unused_references, bool, "Flag to determine whether or not references contained in the " "reference table are allowed to be ignored by the cost."); @@ -165,14 +170,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); "If the peak magnitude of the reference generalized force data is " "close to zero, an exception is thrown (default: false)."); OpenSim_DECLARE_PROPERTY(ignore_constrained_coordinates, bool, - "Flag to determine whether or not to ignore coordinates that are " - "locked, prescribed, or coupled to other coordinates " - "(default: true)."); + "Flag to determine whether or not to ignore generalized forces for " + "coordinates that are locked, prescribed, or coupled to other " + "coordinates (default: true)."); mutable GCVSplineSet m_refsplines; mutable std::vector m_generalizedForceWeights; mutable std::vector m_generalizedForceNames; - mutable std::vector m_coordinateIndexes; + mutable std::vector m_generalizedForceIndexes; mutable std::vector m_normalizationFactors; mutable SimTK::Array_ m_forceIndexes; }; diff --git a/OpenSim/Moco/MocoGoal/MocoGoal.cpp b/OpenSim/Moco/MocoGoal/MocoGoal.cpp index 5a7175cca6..98003bc3b7 100644 --- a/OpenSim/Moco/MocoGoal/MocoGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoGoal.cpp @@ -45,7 +45,7 @@ void MocoGoal::printDescription() const { if (mode == "cost") { str += fmt::format(", weight: {}", get_weight()); } - log_cout(str); + log_info(str); printDescriptionImpl(); } diff --git a/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp b/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp index 40a873d746..604c18f7be 100644 --- a/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp @@ -154,15 +154,15 @@ void MocoJointReactionGoal::calcIntegrandImpl( } void MocoJointReactionGoal::printDescriptionImpl() const { - log_cout(" joint path: ", get_joint_path()); - log_cout(" loads frame: ", get_loads_frame()); - log_cout(" expressed: ", get_expressed_in_frame_path()); + log_info(" joint path: ", get_joint_path()); + log_info(" loads frame: ", get_loads_frame()); + log_info(" expressed: ", get_expressed_in_frame_path()); std::vector measures(getProperty_reaction_measures().size()); for (int i = 0; i < (int)measures.size(); i++) { measures[i] = get_reaction_measures(i); } - log_cout(" reaction measures: {}", fmt::join(measures, ", ")); + log_info(" reaction measures: {}", fmt::join(measures, ", ")); - log_cout(" reaction weights: {}", fmt::join(m_measureWeights, ", ")); + log_info(" reaction weights: {}", fmt::join(m_measureWeights, ", ")); } diff --git a/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp b/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp index b19309ebd1..cacf03eb92 100644 --- a/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp @@ -40,6 +40,6 @@ void MocoMarkerFinalGoal::constructProperties() { } void MocoMarkerFinalGoal::printDescriptionImpl() const { - log_cout(" point name: {}", get_point_name()); - log_cout(" reference location: {}", get_reference_location()); + log_info(" point name: {}", get_point_name()); + log_info(" reference location: {}", get_reference_location()); } diff --git a/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp index 7489aa1002..0164d7cdc4 100644 --- a/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp @@ -158,12 +158,12 @@ void MocoMarkerTrackingGoal::calcIntegrandImpl( } void MocoMarkerTrackingGoal::printDescriptionImpl() const { - log_cout( + log_info( " allow unused references: ", get_allow_unused_references()); - log_cout(" tracked marker(s):"); + log_info(" tracked marker(s):"); int weightIndex = 0; for (const auto& name : m_marker_names) { - log_cout(" {}, weight: {}", name, + log_info(" {}, weight: {}", name, m_marker_weights[weightIndex]); weightIndex++; } diff --git a/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp index c84aad9538..9fc2cb4c2b 100644 --- a/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp @@ -222,10 +222,10 @@ void MocoOrientationTrackingGoal::calcIntegrandImpl( } void MocoOrientationTrackingGoal::printDescriptionImpl() const { - log_cout(" rotation reference file: {}", + log_info(" rotation reference file: {}", get_rotation_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_rotation_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp b/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp index cdd4ae9f9a..87eb56e81c 100644 --- a/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp @@ -260,7 +260,7 @@ void MocoOutputBase::printDescriptionImpl() const { str += fmt::format(", bounds: {}", getConstraintInfo().getBounds()[0]); } - log_cout(str); + log_info(str); } // ============================================================================ diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp index 31b6e1c531..3cdc4fd894 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp @@ -156,14 +156,14 @@ void MocoPeriodicityGoal::calcGoalImpl( } void MocoPeriodicityGoal::printDescriptionImpl() const { - log_cout(" state periodicity pairs:"); + log_info(" state periodicity pairs:"); for (const auto& pair : m_state_names) { - log_cout(" initial: {}, final: {}", pair.first, + log_info(" initial: {}, final: {}", pair.first, pair.second); } - log_cout(" control periodicity pairs:"); + log_info(" control periodicity pairs:"); for (const auto& pair : m_control_names) { - log_cout(" initial: {}, final: {}", pair.first, + log_info(" initial: {}, final: {}", pair.first, pair.second); } } diff --git a/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp index ed42305a89..ff2f62863b 100644 --- a/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp @@ -168,8 +168,7 @@ void MocoStateTrackingGoal::calcIntegrandImpl( void MocoStateTrackingGoal::printDescriptionImpl() const { for (int i = 0; i < (int) m_state_names.size(); i++) { - log_cout(" state: {}, weight: {}", m_state_names[i], + log_info(" state: {}, weight: {}", m_state_names[i], m_state_weights[i]); } } - diff --git a/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp b/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp index 972e549c4e..6d04b6d5de 100644 --- a/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp @@ -120,7 +120,7 @@ void MocoStepLengthAsymmetryGoal::calcGoalImpl(const GoalInput& input, } void MocoStepLengthAsymmetryGoal::printDescriptionImpl() const { - log_cout(" target asymmetry: ", get_target_asymmetry()); - log_cout(" left foot frame: ", get_left_foot_frame()); - log_cout(" right foot frame: ", get_right_foot_frame()); + log_info(" target asymmetry: ", get_target_asymmetry()); + log_info(" left foot frame: ", get_left_foot_frame()); + log_info(" right foot frame: ", get_right_foot_frame()); } \ No newline at end of file diff --git a/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp b/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp index dbb00ba796..16737f46c4 100644 --- a/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp @@ -228,22 +228,22 @@ void MocoStepTimeAsymmetryGoal::calcGoalImpl(const GoalInput& input, } void MocoStepTimeAsymmetryGoal::printDescriptionImpl() const { - log_cout(" target asymmetry: {}", get_target_asymmetry()); + log_info(" target asymmetry: {}", get_target_asymmetry()); const auto& leftGroup = get_left_contact_group(); - log_cout(" left forces:"); + log_info(" left forces:"); for (int ic = 0; ic < leftGroup.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", leftGroup.get_contact_force_paths(ic)); + log_info(" {}", leftGroup.get_contact_force_paths(ic)); } - log_cout(" left contact sphere for position: {}", + log_info(" left contact sphere for position: {}", leftGroup.get_foot_position_contact_force_path()); const auto& rightGroup = get_right_contact_group(); - log_cout(" right forces:"); + log_info(" right forces:"); for (int ic = 0; ic < leftGroup.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", rightGroup.get_contact_force_paths(ic)); + log_info(" {}", rightGroup.get_contact_force_paths(ic)); } - log_cout(" right contact sphere for position: {}", + log_info(" right contact sphere for position: {}", rightGroup.get_foot_position_contact_force_path()); } diff --git a/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp b/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp index 2815f7ba9b..ab09830657 100644 --- a/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp @@ -104,7 +104,7 @@ void MocoSumSquaredStateGoal::calcIntegrandImpl( void MocoSumSquaredStateGoal::printDescriptionImpl() const { for (int i = 0; i < (int)m_state_names.size(); i++) { - log_cout(" state: {}, weight: {}", m_state_names[i], + log_info(" state: {}, weight: {}", m_state_names[i], m_state_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp index 17089bd48d..6e0efc9066 100644 --- a/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp @@ -162,10 +162,10 @@ void MocoTranslationTrackingGoal::calcIntegrandImpl( } void MocoTranslationTrackingGoal::printDescriptionImpl() const { - log_cout(" translation reference file: {}", + log_info(" translation reference file: {}", get_translation_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", i, m_frame_paths[i], + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_translation_weights[i]); } } diff --git a/OpenSim/Moco/MocoInverse.cpp b/OpenSim/Moco/MocoInverse.cpp index aecf680196..e0d092bec0 100644 --- a/OpenSim/Moco/MocoInverse.cpp +++ b/OpenSim/Moco/MocoInverse.cpp @@ -42,6 +42,10 @@ void MocoInverse::constructProperties() { MocoStudy MocoInverse::initialize() const { return initializeInternal().first; } +TimeSeriesTable MocoInverse::initializeKinematics() const { + return initializeInternal().second; +} + std::pair MocoInverse::initializeInternal() const { // Process inputs. @@ -100,7 +104,7 @@ std::pair MocoInverse::initializeInternal() const { // ------------------------- auto& solver = study.initCasADiSolver(); solver.set_multibody_dynamics_mode("implicit"); - solver.set_interpolate_control_midpoints(false); + solver.set_interpolate_control_mesh_interior_points(false); solver.set_minimize_implicit_auxiliary_derivatives(true); solver.set_implicit_auxiliary_derivatives_weight(0.01); solver.set_optim_convergence_tolerance(get_convergence_tolerance()); diff --git a/OpenSim/Moco/MocoInverse.h b/OpenSim/Moco/MocoInverse.h index 41686bf047..f430bbde35 100644 --- a/OpenSim/Moco/MocoInverse.h +++ b/OpenSim/Moco/MocoInverse.h @@ -199,6 +199,7 @@ class OSIMMOCO_API MocoInverse : public MocoTool { } MocoStudy initialize() const; + TimeSeriesTable initializeKinematics() const; /// Solve the problem returned by initialize() and compute the outputs /// listed in output_paths. MocoInverseSolution solve() const; diff --git a/OpenSim/Moco/MocoOutputConstraint.cpp b/OpenSim/Moco/MocoOutputConstraint.cpp index 415041b62e..6a299b3a1c 100644 --- a/OpenSim/Moco/MocoOutputConstraint.cpp +++ b/OpenSim/Moco/MocoOutputConstraint.cpp @@ -265,5 +265,5 @@ void MocoOutputConstraint::printDescriptionImpl() const { // Exponent. str += fmt::format(", exponent: {}", getExponent()); - log_cout(str); + log_info(str); } \ No newline at end of file diff --git a/OpenSim/Moco/MocoParameter.cpp b/OpenSim/Moco/MocoParameter.cpp index 9698ecfb97..4a1c4c9a71 100644 --- a/OpenSim/Moco/MocoParameter.cpp +++ b/OpenSim/Moco/MocoParameter.cpp @@ -140,7 +140,7 @@ void MocoParameter::printDescription() const { fmt::format("{}", getProperty_property_element().getValue()); } - log_cout(" {}. model property name: {}. component paths: {}. " + log_info(" {}. model property name: {}. component paths: {}. " "property element: {}. bounds: {}", getName(), getPropertyName(), fmt::join(componentPaths, ", "), propertyElementStr, getBounds()); diff --git a/OpenSim/Moco/MocoParameter.h b/OpenSim/Moco/MocoParameter.h index 60456bd14b..aaa47c3e53 100644 --- a/OpenSim/Moco/MocoParameter.h +++ b/OpenSim/Moco/MocoParameter.h @@ -20,8 +20,8 @@ #include "MocoBounds.h" -#include #include +#include #include namespace OpenSim { @@ -141,6 +141,9 @@ class OSIMMOCO_API MocoParameter : public Object { { set_property_name(propertyName); } void appendComponentPath(const std::string& componentPath) { append_component_paths(componentPath); } + int getPropertyElement() const { + return get_property_element(); + } /** For use by solvers. This performs error checks and caches information about the model that is useful during the optimization. diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index f0e5a2f091..279cf13fa3 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -1043,7 +1043,7 @@ void MocoProblemRep::printDescription() const { } else { ss << "(total: " << size << ")"; } - log_cout(ss.str()); + log_info(ss.str()); }; printHeaderLine("Costs", m_costs.size()); diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index 2e627884d0..66ee12ebb6 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -266,7 +266,7 @@ class OSIMMOCO_API MocoProblemRep { } /// Print a description of this problem, including costs and variable - /// bounds. Printing is done using OpenSim::log_cout(). + /// bounds. Printing is done using OpenSim::log_info(). void printDescription() const; /// @name Interface for solvers diff --git a/OpenSim/Moco/MocoStudy.cpp b/OpenSim/Moco/MocoStudy.cpp index f5b68e5487..cabca37633 100644 --- a/OpenSim/Moco/MocoStudy.cpp +++ b/OpenSim/Moco/MocoStudy.cpp @@ -108,12 +108,6 @@ void MocoStudy::visualize(const MocoTrajectory& trajectory) const { VisualizerUtilities::showMotion(model, trajectory.exportToStatesTable()); } -TimeSeriesTable MocoStudy::analyze(const MocoTrajectory& trajectory, - const std::vector& outputPaths) const { - return OpenSim::analyzeMocoTrajectory( - get_problem().createRep().getModelBase(), trajectory, outputPaths); -} - TimeSeriesTable MocoStudy::calcGeneralizedForces( const MocoTrajectory& trajectory, const std::vector& forcePaths) const { diff --git a/OpenSim/Moco/MocoStudy.h b/OpenSim/Moco/MocoStudy.h index 561cf5a5fc..3ba6a0f8b9 100644 --- a/OpenSim/Moco/MocoStudy.h +++ b/OpenSim/Moco/MocoStudy.h @@ -19,6 +19,8 @@ * -------------------------------------------------------------------------- */ #include "MocoSolver.h" +#include "MocoProblem.h" +#include "MocoUtilities.h" #include #include @@ -162,14 +164,24 @@ class OSIMMOCO_API MocoStudy : public Object { /// ".*activation" gives the activation of all muscles. /// Constraints are not enforced but prescribed motion (e.g., /// PositionMotion) is. - /// @see OpenSim::analyze() + /// @see OpenSim::analyze() OpenSim::analyzeMocoTrajectory() /// @note Parameters in the MocoTrajectory are **not** applied to the model. /// @note If the MocoTrajectory was generated from a MocoStudy with /// Controller%s in the model, first call /// MocoTrajectory::generateControlsFromModelControllers() to populate /// the trajectory with the correct model controls. + template + TimeSeriesTable_ analyze(const MocoTrajectory& traj, + const std::vector& outputPaths) const { + return OpenSim::analyzeMocoTrajectory( + get_problem().createRep().getModelBase(), traj, outputPaths); + } + + // @copydoc analyze() TimeSeriesTable analyze(const MocoTrajectory& traj, - const std::vector& outputPaths) const; + const std::vector& outputPaths) const { + return analyze(traj, outputPaths); + } /// Compute the generalized coordinate forces for the provided trajectory /// based on a set of applied model Force%s. This can be used to compute diff --git a/OpenSim/Moco/MocoStudyFactory.cpp b/OpenSim/Moco/MocoStudyFactory.cpp index fc26f48c0d..198b11818c 100644 --- a/OpenSim/Moco/MocoStudyFactory.cpp +++ b/OpenSim/Moco/MocoStudyFactory.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace OpenSim; @@ -38,22 +39,29 @@ MocoStudy MocoStudyFactory::createLinearTangentSteeringStudy( double computeActuation(const SimTK::State&) const override { return SimTK::NaN; } - void computeForce(const SimTK::State& state, - SimTK::Vector_&, - SimTK::Vector& mobilityForces) const override { - const double angle = getControl(state); - const auto& coordX = getModel().getCoordinateSet().get(0); - const auto& coordY = getModel().getCoordinateSet().get(1); - applyGeneralizedForce(state, coordX, - get_acceleration() * cos(angle), mobilityForces); - applyGeneralizedForce(state, coordY, - get_acceleration() * sin(angle), mobilityForces); - } double getSpeed(const SimTK::State& state) const override { return SimTK::NaN; } + private: + void implProduceForces(const SimTK::State& state, + ForceConsumer& forceConsumer) const override { + const double angle = getControl(state); + const auto& coordX = getModel().getCoordinateSet().get(0); + const auto& coordY = getModel().getCoordinateSet().get(1); + + forceConsumer.consumeGeneralizedForce( + state, + coordX, + get_acceleration() * cos(angle) + ); + forceConsumer.consumeGeneralizedForce( + state, + coordY, + get_acceleration() * sin(angle) + ); + } }; class LinearTangentFinalSpeed : public MocoGoal { diff --git a/OpenSim/Moco/MocoTropterSolver.cpp b/OpenSim/Moco/MocoTropterSolver.cpp index bb3b2ec7cd..0f17c0ba58 100644 --- a/OpenSim/Moco/MocoTropterSolver.cpp +++ b/OpenSim/Moco/MocoTropterSolver.cpp @@ -97,6 +97,8 @@ MocoTropterSolver::createTropterSolver( // is set as the transcription scheme. if (getProblemRep().getNumKinematicConstraintEquations()) { + checkPropertyValueIsInSet( + getProperty_kinematic_constraint_method(), {"Posa2016"}); OPENSIM_THROW_IF(get_transcription_scheme() != "hermite-simpson" && get_enforce_constraint_derivatives(), Exception, @@ -370,6 +372,7 @@ MocoSolution MocoTropterSolver::solveImpl() const { !get_minimize_lagrange_multipliers()) { checkConstraintJacobianRank(mocoSolution); } + checkSlackVariables(mocoSolution); // TODO move this to convert(): const long long elapsed = stopwatch.getElapsedTimeInNs(); diff --git a/OpenSim/Moco/MocoUtilities.cpp b/OpenSim/Moco/MocoUtilities.cpp index eacb4b6db4..9c09d89569 100644 --- a/OpenSim/Moco/MocoUtilities.cpp +++ b/OpenSim/Moco/MocoUtilities.cpp @@ -350,26 +350,49 @@ TimeSeriesTable OpenSim::createExternalLoadsTableForGait(Model model, forceValues[11]); } - // Compute centers of pressure for both feet. We need to use the force - // and torque information from the half space to compute the correct - // locations. + // The simulated ground reaction forces and moments are just the + // opposite of the sum of forces and torques applied to the half-space. + SimTK::Vec3 forcesRight = -halfSpaceForcesRight; + SimTK::Vec3 forcesLeft = -halfSpaceForcesLeft; + SimTK::Vec3 momentsRight = -halfSpaceTorquesRight; + SimTK::Vec3 momentsLeft = -halfSpaceTorquesLeft; + + // The ground reaction moments should be equal to the moment caused by + // ground reaction force when applied at the center of pressure plus the + // vertical torque (see http://www.kwon3d.com/theory/grf/cop.html). + // Here, the vertical components of the forces and torques are in the + // y-direction (i.e., the y-component of the center of pressure is zero). // TODO: Support contact plane normals in any direction. + + // Compute centers of pressure. SimTK::Vec3 copRight(0); - copRight(0) = halfSpaceTorquesRight(2) / halfSpaceForcesRight(1); - copRight(2) = -halfSpaceTorquesRight(0) / halfSpaceForcesRight(1); + if (std::abs(forcesRight(1)) > SimTK::SignificantReal) { + copRight(0) = momentsRight(2) / forcesRight(1); + copRight(2) = -momentsRight(0) / forcesRight(1); + } SimTK::Vec3 copLeft(0); - copLeft(0) = halfSpaceTorquesLeft(2) / halfSpaceForcesLeft(1); - copLeft(2) = -halfSpaceTorquesLeft(0) / halfSpaceForcesLeft(1); + if (std::abs(forcesLeft(1)) > SimTK::SignificantReal) { + copLeft(0) = momentsLeft(2) / forcesLeft(1); + copLeft(2) = -momentsLeft(0) / forcesLeft(1); + } + + // Calculate the vertical torques. + SimTK::Vec3 verticalTorqueRight(0); + verticalTorqueRight(1) = momentsRight(1) - copRight(2) * forcesRight(0) + + copRight(0) * forcesRight(2); + SimTK::Vec3 verticalTorqueLeft(0); + verticalTorqueLeft(1) = momentsLeft(1) - copLeft(2) * forcesLeft(0) + + copLeft(0) * forcesLeft(2); // Append row to table. SimTK::RowVector_ row(6); - row(0) = sphereForcesRight; + row(0) = forcesRight; row(1) = copRight; - row(2) = sphereForcesLeft; + row(2) = forcesLeft; row(3) = copLeft; - row(4) = sphereTorquesRight; - row(5) = sphereTorquesLeft; + row(4) = verticalTorqueRight; + row(5) = verticalTorqueLeft; externalForcesTable.appendRow(state.getTime(), row); } // Create table. diff --git a/OpenSim/Moco/MocoUtilities.h b/OpenSim/Moco/MocoUtilities.h index 32a8f81e4c..54638246e5 100644 --- a/OpenSim/Moco/MocoUtilities.h +++ b/OpenSim/Moco/MocoUtilities.h @@ -250,7 +250,9 @@ class FileDeletionThrower { /// distinguish between right ("<>_r") and left ("<>_l") forces, centers of /// pressure, and torques. Centers of pressure are computed assuming the /// that the contact plane's normal is in the y-direction, which is the OpenSim -/// convention. +/// convention. Torques are computed based on the center of pressure +/// representation of the ground reaction forces: a "vertical" torque in the +/// y-direction and zero torque in the other directions. /// /// The forces and torques are computed from the first six outputs of /// getRecordValues(), while the centers of pressure are computed from the second diff --git a/OpenSim/Moco/MocoVariableInfo.cpp b/OpenSim/Moco/MocoVariableInfo.cpp index e5835626c7..25db7604ef 100644 --- a/OpenSim/Moco/MocoVariableInfo.cpp +++ b/OpenSim/Moco/MocoVariableInfo.cpp @@ -74,7 +74,7 @@ void MocoVariableInfo::printDescription() const { if (final.isSet()) { str += fmt::format(" final: {}", final); } - log_cout(str); + log_info(str); } void MocoVariableInfo::constructProperties() { diff --git a/OpenSim/Moco/RegisterTypes_osimMoco.cpp b/OpenSim/Moco/RegisterTypes_osimMoco.cpp index 581d87b663..eba929ae94 100644 --- a/OpenSim/Moco/RegisterTypes_osimMoco.cpp +++ b/OpenSim/Moco/RegisterTypes_osimMoco.cpp @@ -33,6 +33,7 @@ #include "MocoGoal/MocoContactTrackingGoal.h" #include "MocoGoal/MocoControlGoal.h" #include "MocoGoal/MocoControlTrackingGoal.h" +#include "MocoGoal/MocoExpressionBasedParameterGoal.h" #include "MocoGoal/MocoGoal.h" #include "MocoGoal/MocoInitialActivationGoal.h" #include "MocoGoal/MocoInitialForceEquilibriumDGFGoal.h" @@ -81,6 +82,7 @@ OSIMMOCO_API void RegisterTypes_osimMoco() { Object::registerType(MocoControlGoal()); Object::registerType(MocoSumSquaredStateGoal()); Object::registerType(MocoControlTrackingGoal()); + Object::registerType(MocoExpressionBasedParameterGoal()); Object::registerType(MocoInitialActivationGoal()); Object::registerType(MocoInitialVelocityEquilibriumDGFGoal()); Object::registerType(MocoInitialForceEquilibriumDGFGoal()); diff --git a/OpenSim/Moco/Test/CMakeLists.txt b/OpenSim/Moco/Test/CMakeLists.txt index a24687cb39..3705069160 100644 --- a/OpenSim/Moco/Test/CMakeLists.txt +++ b/OpenSim/Moco/Test/CMakeLists.txt @@ -51,7 +51,9 @@ MocoAddTest(NAME testMocoImplicit) MocoAddTest(NAME testMocoConstraints) -MocoAddTest(NAME testMocoContact) +MocoAddTest(NAME testMocoContact RESOURCES + subject_20dof18musc_running.osim + running_solution_full_stride.sto) MocoAddTest(NAME testMocoControllers) diff --git a/OpenSim/Moco/Test/running_solution_full_stride.sto b/OpenSim/Moco/Test/running_solution_full_stride.sto new file mode 100644 index 0000000000..629fb8f529 --- /dev/null +++ b/OpenSim/Moco/Test/running_solution_full_stride.sto @@ -0,0 +1,109 @@ +inDegrees=no +num_controls=25 +num_derivatives=18 +num_multipliers=0 +num_parameters=0 +num_slacks=0 +num_states=87 +DataType=double +version=3 +OpenSimVersion=4.4.1-2023-11-17-f882edf +endheader +time /jointset/groundPelvis/pelvis_tilt/value /jointset/groundPelvis/pelvis_list/value /jointset/groundPelvis/pelvis_rotation/value /jointset/groundPelvis/pelvis_tx/value /jointset/groundPelvis/pelvis_ty/value /jointset/back/lumbar_extension/value /jointset/back/lumbar_bending/value /jointset/back/lumbar_rotation/value /jointset/hip_r/hip_flexion_r/value /jointset/hip_l/hip_flexion_l/value /jointset/walker_knee_r/knee_angle_r/value /jointset/patellofemoral_r/knee_angle_r_beta/value /jointset/walker_knee_l/knee_angle_l/value /jointset/patellofemoral_l/knee_angle_l_beta/value /jointset/acromial_r/arm_flex_r/value /jointset/acromial_l/arm_flex_l/value /jointset/ankle_r/ankle_angle_r/value /jointset/ankle_l/ankle_angle_l/value /jointset/elbow_r/elbow_flex_r/value /jointset/elbow_l/elbow_flex_l/value /jointset/mtp_r/mtp_angle_r/value /jointset/mtp_l/mtp_angle_l/value /jointset/groundPelvis/pelvis_tilt/speed /jointset/groundPelvis/pelvis_list/speed /jointset/groundPelvis/pelvis_rotation/speed /jointset/groundPelvis/pelvis_tx/speed /jointset/groundPelvis/pelvis_ty/speed /jointset/back/lumbar_extension/speed /jointset/back/lumbar_bending/speed /jointset/back/lumbar_rotation/speed /jointset/hip_r/hip_flexion_r/speed /jointset/hip_l/hip_flexion_l/speed /jointset/walker_knee_r/knee_angle_r/speed /jointset/patellofemoral_r/knee_angle_r_beta/speed /jointset/walker_knee_l/knee_angle_l/speed /jointset/patellofemoral_l/knee_angle_l_beta/speed /jointset/acromial_r/arm_flex_r/speed /jointset/acromial_l/arm_flex_l/speed /jointset/ankle_r/ankle_angle_r/speed /jointset/ankle_l/ankle_angle_l/speed /jointset/elbow_r/elbow_flex_r/speed /jointset/elbow_l/elbow_flex_l/speed /jointset/mtp_r/mtp_angle_r/speed /jointset/mtp_l/mtp_angle_l/speed /lumbarExtAct/activation /lumbarBendAct/activation /lumbarRotAct/activation /shoulder_flex_Actu_r/activation /elbow_flex_Actu_r/activation /shoulder_flex_Actu_l/activation /elbow_flex_Actu_l/activation /forceset/bfsh_r/activation /forceset/bfsh_r/normalized_tendon_force /forceset/gasmed_r/activation /forceset/gasmed_r/normalized_tendon_force /forceset/glmax2_r/activation /forceset/glmax2_r/normalized_tendon_force /forceset/psoas_r/activation /forceset/psoas_r/normalized_tendon_force /forceset/recfem_r/activation /forceset/recfem_r/normalized_tendon_force /forceset/semimem_r/activation /forceset/semimem_r/normalized_tendon_force /forceset/soleus_r/activation /forceset/soleus_r/normalized_tendon_force /forceset/tibant_r/activation /forceset/tibant_r/normalized_tendon_force /forceset/vasint_r/activation /forceset/vasint_r/normalized_tendon_force /forceset/bfsh_l/activation /forceset/bfsh_l/normalized_tendon_force /forceset/gasmed_l/activation /forceset/gasmed_l/normalized_tendon_force /forceset/glmax2_l/activation /forceset/glmax2_l/normalized_tendon_force /forceset/psoas_l/activation /forceset/psoas_l/normalized_tendon_force /forceset/recfem_l/activation /forceset/recfem_l/normalized_tendon_force /forceset/semimem_l/activation /forceset/semimem_l/normalized_tendon_force /forceset/soleus_l/activation /forceset/soleus_l/normalized_tendon_force /forceset/tibant_l/activation /forceset/tibant_l/normalized_tendon_force /forceset/vasint_l/activation /forceset/vasint_l/normalized_tendon_force /lumbarExtAct /lumbarBendAct /lumbarRotAct /shoulder_flex_Actu_r /elbow_flex_Actu_r /shoulder_flex_Actu_l /elbow_flex_Actu_l /forceset/bfsh_r /forceset/gasmed_r /forceset/glmax2_r /forceset/psoas_r /forceset/recfem_r /forceset/semimem_r /forceset/soleus_r /forceset/tibant_r /forceset/vasint_r /forceset/bfsh_l /forceset/gasmed_l /forceset/glmax2_l /forceset/psoas_l /forceset/recfem_l /forceset/semimem_l /forceset/soleus_l /forceset/tibant_l /forceset/vasint_l /forceset/bfsh_r/implicitderiv_normalized_tendon_force /forceset/gasmed_r/implicitderiv_normalized_tendon_force /forceset/glmax2_r/implicitderiv_normalized_tendon_force /forceset/psoas_r/implicitderiv_normalized_tendon_force /forceset/recfem_r/implicitderiv_normalized_tendon_force /forceset/semimem_r/implicitderiv_normalized_tendon_force /forceset/soleus_r/implicitderiv_normalized_tendon_force /forceset/tibant_r/implicitderiv_normalized_tendon_force /forceset/vasint_r/implicitderiv_normalized_tendon_force /forceset/bfsh_l/implicitderiv_normalized_tendon_force /forceset/gasmed_l/implicitderiv_normalized_tendon_force /forceset/glmax2_l/implicitderiv_normalized_tendon_force /forceset/psoas_l/implicitderiv_normalized_tendon_force /forceset/recfem_l/implicitderiv_normalized_tendon_force /forceset/semimem_l/implicitderiv_normalized_tendon_force /forceset/soleus_l/implicitderiv_normalized_tendon_force /forceset/tibant_l/implicitderiv_normalized_tendon_force /forceset/vasint_l/implicitderiv_normalized_tendon_force +0 -0.3815779803062549 0 -0.006359962401108366 0 0.9700122027731819 0.3737108923951569 0 -0.02550365577217573 0.7536390229734087 0.2235760077366384 0.324586936318175 0.324586936318175 0.8114684894265207 0.8114684894265207 -0.6080162000402711 -0.2386212498125841 -0.1008472700848131 -0.152386605636023 2.512857819473362 2.517062825013944 0.001147219831816315 -0.02394509717485338 1.419148030660006 -0.07258936671325866 0.9674824471514465 2.701331163558203 -0.3552707236111337 -1.651671173887599 -0.4105952416897347 -0.7968527933910834 -1.601900242266194 1.462569640795185 1.119521044749527 1.119521044749527 7.5593157520969 7.5593157520969 1.724842063566951 -1.221426131452406 0.1102039849331984 1.080244725480482 -0.7095071304475344 -1.73696799100674 0.02046684948361133 0.7297520846527723 -0.001953636701200079 0.1322254358571739 0.2236784889423009 0.2222854778214978 0.09595785989598138 -0.04637074765099625 -0.1267095197725938 0.01000007750874232 0.05062841251703976 0.01396454984184725 0.03324703752757507 0.01000001010315366 0.03574134059991296 0.0100078828185528 0.01317295252509831 0.01000003818867956 0.002968972460700048 0.3510651746588573 0.4459592317820888 0.009999999999999787 0.01965307916341708 0.1494945936072893 0.1186560538583499 0.009999999999999787 0.02095061465181169 0.01022435431045476 0.02473114216490746 0.2871254515724271 0.1409598622863979 0.009999999999999787 0.02753197204759639 0.4613688538923459 0.5022395700726499 0.01000002519298659 0.01600535087674393 0.01003898382910373 0.02580935762917891 0.01000423918461024 0.01862942731983352 0.352799565026769 0.4563202617684325 0.01000000401993262 0.01628247938198513 0.4388549903727527 -0.2123647564909695 -0.03322181483202047 -0.3146275535331956 -0.4360611650504068 -0.1821642547342651 0.3400800821079133 0.01000007750874188 0.01396454984184758 0.01000001010315366 0.01000788281855314 0.01000003818867978 0.3510651746588475 0.01000000000000001 0.1494945936072893 0.01000000000000001 0.01022435431045432 0.2871254515724196 0.01000000000000001 0.461368853892336 0.01000002519298637 0.01003898382910406 0.01000423918460969 0.3527995650267666 0.01000000401993306 0.4078243446262094 -2.257739359552792 -0.5563215245646264 0.6426368747599984 1.972494007904218 -46.34854764203742 -0.8429233450349777 3.731510782808301 -9.510395257309687 -0.7844438499217479 -7.317976920417489 -0.659982585730807 -35.18370285923768 13.92270980127914 -19.62599804150908 -0.7428231223125819 -15.68835827474218 22.82503467996948 +0.00696476042322941 -0.3670096942548957 -0.0006063720507687442 0.0007789945021108302 0.01878124226404054 0.9676259944724022 0.3564431559667063 -0.00286653031827333 -0.03140232694249079 0.7384961377908055 0.2311406162113103 0.3388404412115182 0.3388404412115182 0.8691450442626092 0.8691450442626092 -0.5932094680353164 -0.2456955961653686 -0.09548027692571281 -0.1421100221664213 2.508869997337774 2.503094836843346 0.001459450541191565 -0.01887689120005565 2.516194589834564 -0.1008542513484101 1.074482939584334 2.685129811618171 -0.3490519471136153 -3.016257099388684 -0.4063870219309274 -0.8924022402481255 -2.47986567363054 0.9689577703258756 2.916613519789006 2.916613519788997 8.945653935340827 8.945653935340827 2.458820768020243 -0.8715274212031607 1.365477318214819 1.779989963963319 -0.4886254664554519 -2.22804061967514 0.02893422479889463 0.6885299775841078 0.04366924010509488 0.08404549493473024 0.1793287303291895 0.158492833410619 0.00794385790081531 -0.0543902211905456 -0.06169840460998088 0.01000006789906349 0.04904192198841306 0.01340989204985199 0.02596302681878671 0.01390407278241534 0.03716591040052775 0.01000677145440454 0.01433159381251814 0.01000003258750226 0.006346966806175303 0.3194717032869749 0.2558508789080909 0.009999999999999787 0.01862953678382162 0.139514355382266 0.129861090879984 0.009999999999999787 0.01605094416305697 0.01019275854920654 0.02147283710252967 0.2794206758611399 0.1094523187416874 0.009999999999999787 0.027123627942649 0.4565341301223595 0.4086229219122806 0.01000002046094028 0.04129167165007441 0.01003349094152339 0.002208909733582676 0.01000364055978764 0.01763672004889516 0.3473308440689671 0.3773517135407767 0.01000000208042762 0.03482563641297665 0.07378598646472934 -0.06160243171099822 -0.01150535548381748 0.04158262230438703 -0.3498847090884142 -0.00478949467335188 0.1331164342131258 0.01000004340683902 0.0119822702279877 0.02391330091246036 0.01000393886053075 0.01000001831175945 0.1805325836955641 0.01000000000000001 0.1054655763653873 0.01000000000000001 0.01011218527768498 0.2476041270085925 0.01000000000000001 0.4333938420949675 0.01000000840035431 0.01001948983685141 0.01000211482620483 0.3232149842457597 0.01000000000000001 -0.5772894343583068 -0.1856359401239308 0.7824009783749462 -0.1292631995380543 -0.3663768771243257 -13.37144545980547 0.320746372859343 -0.2709587275673929 4.10254386070639 -0.2983675543944159 -2.542063603013922 0.2990080710234389 0.5026258418740547 -2.638797126966161 6.042169273722015 0.2441531596128908 -7.897345978077432 -8.38570266869092 +0.01392952084645882 -0.3499843024496712 -0.001395349686882152 0.008494841914417961 0.0373084230725147 0.9648840322779821 0.3357448177734024 -0.005574826693029777 -0.03787021990586181 0.7228101796918667 0.2406847030624566 0.3644213661256335 0.3644213661256335 0.9352772623905987 0.9352772623905987 -0.5747165142871431 -0.2516176639257948 -0.08273926447614333 -0.1288568334715698 2.505313376409124 2.48666856933871 0.0009894701760009283 -0.01487103884557062 2.12473030617231 -0.1250263707868786 1.133142275787578 2.628374897979074 -0.4574289808999854 -2.636802479615405 -0.3651567961746673 -0.9603104741951216 -1.757849013893955 2.030996479954697 4.372301997025883 4.372301997025883 9.987443290730276 9.987443290730276 2.783377180136629 -0.8905379106495424 2.227726725736344 1.935006424065637 -0.5856834292066149 -2.442887271692327 -0.2041528149561085 0.4246867110413133 0.01496276043213518 0.07187125872806899 0.1469360122532803 0.1707146923658138 -0.04854416256642713 -0.02885599005343975 -0.04591083163608856 0.01000004530933918 0.04657244192059729 0.01210171038274543 0.02576221347648966 0.0229924177288372 0.04409071059799441 0.01000415892318651 0.01388886226568742 0.0100000194206662 0.006725982790007023 0.2345037998401263 0.1882639261444923 0.009999999999999787 0.02094141695587393 0.1143356473591508 0.1182654125057416 0.009999999999999787 0.02236698810076909 0.01011847147874834 0.01852520131625957 0.2596635490691321 0.09423466848283502 0.009999999999999787 0.02830218559804187 0.4439954218392028 0.4006167145745692 0.01000000933719036 0.03528238502074377 0.01002057823694225 0.01515944607198172 0.01000223335114958 0.01905495929702861 0.3332572982764819 0.3336506501942882 0.009999999999999787 0.02643400742897528 -0.2912830174432942 0.08915989306897321 0.01021110386438562 0.3977927981419696 -0.2637082531264214 0.1725852653875612 -0.07384721368166153 0.01000000930493616 0.01000000000000001 0.03782659172176694 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.06143655912348545 0.01000000000000001 0.01000001624491542 0.2080828024447653 0.01000000000000001 0.405418830297599 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2936304034647526 0.01000000000000001 0.1542651449433404 -0.2237290181355532 1.023124160050566 0.1827867918910323 1.11130764169538 -11.1653129965125 0.114876306470382 -2.815940675906568 -6.289691670653784 -0.695232087519524 -2.640172576366534 -0.2042880136972626 -10.59981608508905 4.935865889731112 -9.130020574134491 -0.05049592814187846 -5.560950369416981 15.09044386676547 +0.02089428126968823 -0.3375596283411135 -0.002660912678425831 0.01569838771709353 0.05538581934396003 0.9612547579353823 0.3198785026903925 -0.007208384251021993 -0.04402549420799096 0.7146147751223833 0.2593105089383774 0.4030253037796996 0.4030253037796996 1.007765703625532 1.007765703625532 -0.5542327765191881 -0.2578676107194031 -0.05857256859264659 -0.1149925265648148 2.500340472662276 2.468672103301477 -0.005923372180192565 -0.01165063812866141 1.463230825859308 -0.2421971783839543 0.9275523634933593 2.565557546975886 -0.5810374373679317 -1.937358046895293 -0.09790270102231835 -0.8011027497833647 -0.6372141278571222 3.252195596476902 6.700605655212167 6.700605655212161 10.73068424084614 10.73068424084596 3.099388848703086 -0.904061749955446 4.917490401342304 2.099989828571792 -0.8498476358205573 -2.727718696261017 -1.627476151122551 0.4606919833580232 0.001414076623879712 0.07069429350878931 0.1200759125891353 0.1901173197182118 -0.06676019482399065 -0.008466411679305619 -0.04368443720028825 0.01000002562695235 0.04345009932851918 0.01101662614487564 0.02614859051801544 0.04934853954458296 0.06520265647234469 0.01000200319079614 0.01309375008552882 0.01000000955166902 0.0093971308470846 0.1450505283260335 0.117647174448857 0.01224755453324011 0.02541240589118798 0.08437650821870069 0.08578537292359467 0.009999999999999787 0.02078573901311875 0.01005714284866599 0.01556301411788663 0.2279992924530783 0.07607056379210286 0.009999999999999787 0.02958213600670723 0.4042736791588131 0.3292901267120003 0.01000000054337535 0.04334727441759378 0.01000991991681621 0.00383955324636176 0.01000107202290712 0.01939184807366523 0.3046569208868419 0.2976977189779273 0.009999999999999787 0.03768619039232224 0.1575403720479601 0.04293528591925433 -0.01326139970121842 0.1656353314452749 -0.0419177285425017 0.0008160982506359993 0.004337315353127424 0.01000000562258663 0.01000000000000001 0.109038303772794 0.01000000000000001 0.01000000197795026 0.01000000000000001 0.01799531992413506 0.03571827596156441 0.01000000000000001 0.01000002497361452 0.1484129654918217 0.01000000000000001 0.2518564530303651 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2170529759074797 0.01000000000000001 -0.787822962729989 0.3167779150556416 4.717513692374877 -0.2684714809736695 -0.004465871874462367 -8.980791319270056 1.147358089922605 -6.035547447671064 3.040722384888101 -0.2670645648385561 -2.545658561403444 0.3729371246402571 -10.20510248171877 -1.102311448293948 2.750187660764438 0.1269905298946672 -5.061548081479452 -5.701922276429958 +0.02785904169291764 -0.3293223152078144 -0.004822036449367406 0.02130550762296846 0.07308481558716817 0.9568422169303763 0.3085077443959561 -0.006854531095179972 -0.04894374237291066 0.7133536285314825 0.2850753400823303 0.4575821396402633 0.4575821396402633 1.083390376631768 1.083390376631768 -0.5315343517461475 -0.2642089371273597 -0.01137831202547268 -0.09885664458436727 2.493370761140942 2.448634555425308 -0.01954283723871875 -0.009002522146571934 0.9222880911011138 -0.3821955266465631 0.6747127535139774 2.519713578637891 -0.6823517678500437 -1.345858781476577 0.2055478923042364 -0.6050861557624825 0.2333919067692651 4.081038357743684 8.953334864467926 8.953334864467926 10.88804333459147 10.88804333459147 3.419346508720333 -0.9167784727992707 8.840319057507655 2.587311725650646 -1.159086576807186 -3.029004155809955 -2.130026009409244 0.2603494761517195 0.07154812499605878 0.06136434939509394 0.09382891936788607 0.1639911679904089 -0.04153847299210245 -0.0228548178884771 -0.0277107067950495 0.01000001442404219 0.03926265907544213 0.01050370990706018 0.02992539642772529 0.09587632004217195 0.105319967056428 0.0100009909951293 0.01213608524256493 0.01000000679063406 0.01139695861496604 0.08599732096235702 0.06500723299035771 0.01750081964768491 0.03662192998313074 0.05506119364698403 0.04081629849262258 0.009999999999999787 0.02579120491985609 0.01002831170267005 0.01324953740687151 0.1839744957691734 0.0591951403082418 0.009999999999999787 0.03072642072172815 0.3234797865759753 0.253968331382612 0.009999999999999787 0.04106773364350413 0.01000490962544109 0.00987979816281026 0.01000052641182858 0.02054185457478486 0.2573929948522151 0.2589911219350922 0.009999999999999787 0.03277783761474318 0.6063637615392147 -0.003289321230464326 -0.03673390326682247 -0.06652213525141992 0.1798727960414181 -0.1709530688862893 0.08252184438791621 0.0100000019402372 0.01000000000000001 0.1802500158238211 0.01000000000000001 0.01000000552106151 0.01000000000000001 0.02599064954691965 0.01000000000000001 0.01000000085749975 0.01000003370231362 0.08874312853887834 0.01000000000000001 0.09829407576313109 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1404755483502068 0.01000000000000001 -0.151588207204738 0.7498662419494966 6.480703044055116 0.1361081846992573 0.9185313658227837 -6.003093725500717 2.049913478738087 -6.402290841885046 -4.398251161411229 -0.5089484401636039 -2.27011499253159 -0.243245762569965 -11.74707951584641 1.965360217672229 -4.144884923480984 0.182998887317113 -6.351692809808636 10.44978582715991 +0.03482380211614705 -0.3232952613150535 -0.00765149536386911 0.02616371368347137 0.09057422147312932 0.9519043204705424 0.2995870628037007 -0.005271334642407588 -0.05342595773162895 0.7147121564691066 0.3142216405465219 0.519969445159953 0.519969445159953 1.158333282007499 1.158333282007499 -0.5065176695632221 -0.2706437881441897 0.05139277682135068 -0.08033047012942829 2.484206609375257 2.426844272070052 -0.03485901831721661 -0.007038406936360708 0.8409540796746913 -0.4220315591181278 0.7345336754104803 2.50457999843564 -0.7305763851391238 -1.251191041947434 0.2337296866705687 -0.6948418352897985 0.09395301649648413 4.23635560264943 8.79407165212948 8.794071652129494 10.58071565756388 10.58071565756389 3.761982049407257 -0.9315908675538329 8.832436219435579 2.65570059865075 -1.469544151833194 -3.210358138202098 -2.314541275386113 0.2744032734728341 0.1461041806136358 0.05075256460010502 0.07034165709715756 0.1280236253319593 -0.0080244716115212 -0.04151416535368702 -0.00293834921569136 0.01000000813294122 0.03639877229408439 0.01024305080463339 0.03279348382670344 0.127165846340795 0.1333591871821809 0.01000047673683779 0.01230046719859201 0.01000000489012765 0.01423082978945001 0.05043649771055581 0.03892096475439821 0.03729561749594623 0.05734428604977149 0.0331277271623569 0.01789796084943118 0.01340166111981578 0.02824680907108856 0.01001367494216954 0.01133893604670311 0.1417747301848453 0.04409588542094589 0.009999999999999787 0.03132792263435213 0.2593225155649859 0.18985534202834 0.009999999999999787 0.04433076796238389 0.01000236505654284 0.004989810954512119 0.01000024932369126 0.0215585621505463 0.209993025102249 0.2150374242227566 0.009999999999999787 0.03973365606152468 0.37000008156145 0.008088172315415587 -0.0358904459674414 -0.005872727666101474 0.1105033844280782 -0.08413514178114062 0.1349712240828451 0.0100000025562379 0.01000000000000001 0.1535034552678421 0.01000000000000001 0.0100000011097936 0.01000000000000001 0.08316483706491273 0.01000000000000001 0.02211614021090769 0.0100000560186474 0.07563952196407864 0.01000000000000001 0.1870590677125002 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1273723202108483 0.01000000000000001 -0.5158638315219014 0.1354048477049053 1.861976255945585 -0.02179169625683448 0.09896494282667348 -1.96892057959297 3.684865208234069 -1.007428685300639 3.084124847068298 -0.1346811888084966 -2.060038516112899 0.2466836302156407 -6.473744383424955 -0.4267716261086164 1.29053076509717 0.1068232197462706 -6.051789288694624 -4.112257978364855 +0.04178856253937646 -0.3171553248193701 -0.0105853961482314 0.03173451303424635 0.1080007934637619 0.9467358116214797 0.290586319407931 -0.003812648169698285 -0.05880106881759328 0.7137879952147692 0.3433578544641449 0.5777429910088432 0.5777429910088432 1.230052580379877 1.230052580379877 -0.4791660072407535 -0.2771930467283301 0.1067419500772058 -0.06293605608356723 2.472941755773632 2.404165611092669 -0.05242925651617192 -0.005587827295533643 0.954703449566674 -0.4121870449520206 0.8793409810396073 2.501670202695168 -0.748574401570496 -1.368849322025966 0.1697936172115284 -0.8614900154763425 -0.4221049238975354 4.078141905928382 7.628448036959782 7.628448036959782 9.962352626667153 9.962352626667153 4.089859203391835 -0.9496317399176188 6.709042419187632 2.262315504575972 -1.76232393410439 -3.284110596851797 -2.777300856869079 0.1128826465221664 0.164388211023569 0.04412052468263461 0.05125790707774192 0.1095423688020607 0.006869103509133723 -0.0410836347124337 0.02684315351536437 0.0100000055023699 0.03423518311807161 0.01012036522241022 0.03267057357785541 0.1317125254705291 0.1353090488550341 0.01000023397843641 0.01276738344949546 0.01000000165900161 0.01561324419505805 0.03075287999438681 0.03087984622784123 0.07463946765984009 0.08494363172191832 0.0216675120604175 0.01524280757386132 0.0213300131600529 0.0406240197817076 0.01000680647020236 0.01005044608615613 0.1101888875663852 0.03057984683566151 0.009999999999999787 0.03180448568950123 0.2500598541286001 0.1664367851280728 0.009999999999999787 0.04350267396588858 0.01000116988654209 0.007656436438981729 0.01000011898566511 0.02200010362033389 0.1742312535113721 0.1777328252683299 0.009999999999999787 0.03595150689736881 0.1336364015836854 0.01946566586129539 -0.03504698866806033 0.05477667991921686 0.04113397281473818 0.002682785324008008 0.1874206037777741 0.0100000031722387 0.01000000000000001 0.1267568947118629 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1403390245829058 0.01000000179610794 0.0342322795643154 0.01000007833498118 0.06253591538927916 0.01000000000000001 0.2758240596618695 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1142690920714897 0.01000000000000001 0.04950919246973104 -0.1090283836058995 -1.011114870207653 0.2229839078028129 0.5017311659106927 -0.821250097537856 4.024709217992956 -0.5835165735848165 -1.54916347935531 -0.3303029845296392 -1.81547873633774 -0.2791231940997259 -0.06126981587952809 0.7905521649130246 -1.974928677008215 0.0178342381418212 -4.442338186165789 7.366271980257276 +0.04875332296260586 -0.3105866298303956 -0.01342159550468436 0.03824053007070471 0.1253791116871605 0.9414512232873533 0.2811494045361589 -0.002791968699699332 -0.06528069719403051 0.7101480280745105 0.3718211951438062 0.6291064137683189 0.6291064137683189 1.297275344586919 1.297275344586919 -0.4496712557308213 -0.2839104367732119 0.1512043241272378 -0.04846097670169591 2.459653710003945 2.381355477230234 -0.07286771133514236 -0.004453425398968758 0.9137921674436571 -0.400662319565785 0.9949109211628961 2.488383143741967 -0.7672487329683542 -1.320707691745018 0.11782754430901 -1.005576828902464 -0.6238027223654206 4.115297550560204 7.096708501422869 7.096708501422871 9.331778832411961 9.331778832412015 4.371409331810509 -0.9855732753738558 6.117200906879392 1.908427387729509 -2.052732421532371 -3.250829902822831 -3.033967886022385 0.1809397993030193 0.151906805445007 0.03958273897819353 0.03630553908812351 0.09835933416472731 0.02078868067687267 -0.03596601422951418 0.05768826332654875 0.01000000490713804 0.03238656921844774 0.01394675729411432 0.0341257257989791 0.1159524457436243 0.1223125204075233 0.01000011013601743 0.01309281584412325 0.009999999999999787 0.0174040680602956 0.02031572217120114 0.02644313584422253 0.1123561000144901 0.1138531076211575 0.01572498046092896 0.01220832658831261 0.05459577456678311 0.07064089454325373 0.01281636385355789 0.009888838075722273 0.0903690157398862 0.01934206904041336 0.009999999999999787 0.03232072783317763 0.2456398281670098 0.1636552069178716 0.01000000167559545 0.04472388558769147 0.01891347964791068 0.007353697095786149 0.01000005285808081 0.02215145267786056 0.1478018386643409 0.1509869543788636 0.009999999999999787 0.04094824896310012 0.05908532494442231 0.01852173645400912 -0.02832362253687754 0.04081273323178092 0.1243340741354894 -0.02724031513404279 0.2074488627911055 0.01000000534197798 0.02385953403609786 0.06837844794949755 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1723635950480218 0.01000000231314946 0.1354577833913885 0.02001323595854154 0.06872440779546907 0.01000000000000001 0.2051470641055524 0.01000001382780824 0.04194220072951749 0.01000000000000001 0.1070762949189231 0.01000000000000001 -0.4185004317335182 0.4990169971232392 -2.862041372008123 -0.04525333687356645 0.1037469099007952 -0.4804176985766283 4.253376614962781 -0.3256687313236011 8.601519137146349 0.1990954642428659 -1.391106560379898 0.2572918735210213 -1.041580345179783 -0.2102612726053293 1.198979090677492 0.02463037680194216 -3.285369898465484 -2.886112467041445 +0.05571808338583528 -0.3046741690794144 -0.01614423502306828 0.0456765082480679 0.1426585575591184 0.9360720677857297 0.2724729130780745 -0.002247659849702988 -0.07289710524284843 0.7050895983865613 0.4009593004763885 0.6762574239509882 0.6762574239509882 1.359906737705384 1.359906737705384 -0.4183919594160153 -0.2910085286364659 0.1927655552231577 -0.03615644446632915 2.444358452758234 2.359095053722224 -0.09388534525558079 -0.003512224291103205 0.7662592825845085 -0.3795772116880003 1.14638854172641 2.473278026816516 -0.7757303012023757 -1.150486200368845 0.03299907122507051 -1.187927567752379 -0.8294305062235181 4.271907265265282 6.418812006190249 6.418812006190249 8.64389490248098 8.64389490248098 4.602308935019728 -1.058954041116773 5.875950459695412 1.639021373935079 -2.338714607735836 -3.126255819075829 -2.943617877491627 0.05740312476919485 0.1281916771581972 0.03569538986240328 0.02527616895678797 0.08667299422115926 0.04724704183030415 -0.03719026306571038 0.08657567785200504 0.01000000577724691 0.03066025706883257 0.02297096663970155 0.03923338687468725 0.08012354892586426 0.0934770870766215 0.01000005169379747 0.01331099827168636 0.009999999999999787 0.0183291129582952 0.01514548875884003 0.02380309071396791 0.1456597937895436 0.1438629955347674 0.01284560059770179 0.01017980647608141 0.1177616767049998 0.1386074960303381 0.01938056924431475 0.01164252465836846 0.08213553528301665 0.01148731344718001 0.009999999999999787 0.03301936979513531 0.2152332852292806 0.1476922085315597 0.01000001267105244 0.04377216278037199 0.0394285844869624 0.01475999210923007 0.01000002199190231 0.02232930847636094 0.1288673145215458 0.131309409478753 0.009999999999999787 0.0381688502078954 -0.01546575169484093 0.01757780704672296 -0.02160025640569485 0.02684878654434519 0.2075341754562406 -0.05716341559209359 0.2274771218044368 0.01000000751171737 0.03771907612335113 0.0100000011871324 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2043881655131379 0.01000000283019087 0.2366832872184619 0.03002639358210202 0.07491290020165919 0.01000000000000001 0.1344700685492355 0.01000003003833738 0.07388440321032586 0.01000000000000001 0.09988349776635652 0.01000000000000001 0.08462935238842538 0.9398257429907743 -5.559413424664347 0.1921860174528783 0.2531141527342643 -0.3053198589675419 4.340703436848836 -0.2946449363142297 9.348476763109508 0.2196928449498683 -0.8440103738149008 -0.2267452781140235 -3.846445292361411 0.1665725024145857 0.2387969613976777 0.02544591157533284 -2.412598307202823 5.133276057112723 +0.06268284380906469 -0.2997078488688105 -0.01869129953305748 0.05425763861579203 0.1598559665329535 0.9307218978378318 0.2649120340233373 -0.002372316525744544 -0.08187788257989315 0.6976623397505866 0.4309765671396493 0.7173691682037826 0.7173691682037826 1.417418171557934 1.417418171557934 -0.3856450950941088 -0.2987165359965855 0.2330376620034862 -0.02564607384646322 2.427178639140918 2.337926551227994 -0.1138147974653734 -0.002794747334796011 0.6864995516225991 -0.3446768222209498 1.319346182724522 2.467176834957386 -0.753745455725259 -1.050791075887141 -0.07536249012883101 -1.390415097764314 -1.368127004127031 4.30777810869586 5.339169339351422 5.339169339351341 7.849870469074018 7.849870469074048 4.792812236090506 -1.155588688807937 5.73687358449804 1.370398524334407 -2.585675826482234 -2.941599297892369 -2.766125705283646 0.120252668330552 0.0920612033229502 0.03278732672411433 0.01792860714897193 0.06869544076836753 0.08030461999887972 -0.04187318540257801 0.1169983338488101 0.01000001050327937 0.02983973646203175 0.03571219352068988 0.04705535386227044 0.04707033027410201 0.06387593069935527 0.01000002185237925 0.01396978617077815 0.009999999999999787 0.01928794944749068 0.012500481655767 0.02199165882104515 0.1802281252624378 0.1744715913165247 0.01137802800461829 0.0086016505620643 0.2034308531723825 0.2144203425477516 0.03215206432843409 0.01599700010681548 0.07661376994561575 0.006954064198154697 0.009999999999999787 0.0335721927403565 0.1883671267930289 0.125101908291418 0.01000001966693143 0.04344636510496036 0.06186021674295805 0.02008617072479568 0.01000000640336074 0.02246250751389445 0.1161270811487061 0.1173368837123085 0.009999999999999787 0.0417884721231081 -0.1251941694474826 0.02147195515012079 -0.009625202763107021 -0.05037827682912277 0.2519240507026398 -0.06875772101468941 0.2811610834416388 0.01000002109561482 0.0574642595755972 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2501952616278776 0.01000000034082715 0.398772880528043 0.0571789057751233 0.06617436637877794 0.01000000000000001 0.1582247558277851 0.01000002294911517 0.09586677980839653 0.01000000000000001 0.09930644639895647 0.01000000000000001 -0.2005927293971567 1.326088942248821 -3.124942729860336 0.05701888099481078 0.07085693450138766 -0.2253786580887636 4.489533766811449 -0.1850702774169994 12.10284786218296 0.9671651942266976 -0.4889177216709981 0.2290337963118615 -2.471434410301357 -0.1805499068147355 1.111124831218571 0.01068601313035195 -1.606087697679969 -1.994893025975507 +0.0696476042322941 -0.2947406056387383 -0.02084567162676665 0.06407635017903202 0.1770536818063588 0.9256685700987495 0.2574166753897349 -0.003388898649606453 -0.09225688989253511 0.6851303561522881 0.4604064250603153 0.7499655638203366 0.7499655638203366 1.468956185215308 1.468956185215308 -0.3517483025398409 -0.3071208150484348 0.273350102901333 -0.01718915492373618 2.408466082690779 2.318271680612206 -0.1322323343565159 -0.00223240999245089 0.7665243368080947 -0.2668116914321432 1.501772250654915 2.473365983775956 -0.6904891182212183 -1.131671361946172 -0.2231261631334416 -1.589443166455408 -2.295302884384435 4.103227021564457 3.973545229223672 3.973545229223672 6.928568076510938 6.928568076510938 4.932522226646591 -1.258895359814373 5.887532801416878 1.049352362411882 -2.77885690687826 -2.691601419945969 -2.509456405523286 0.01285324665950593 0.04261055427645477 0.03111036653188304 0.0140781623757249 0.03999457343658541 0.1154131818581843 -0.04780696922821637 0.1516309534706526 0.01000001992367849 0.02953286461251281 0.05166200577559943 0.05798040832757767 0.02896013004501929 0.04738383759155296 0.01000000770519138 0.01494139720013843 0.009999999999999787 0.01999353216148414 0.01124009612671495 0.02051702551306311 0.2179934841251518 0.206966639812344 0.01068293310169821 0.007232286774680574 0.3110474366724985 0.3027496551220583 0.05181747872517795 0.02422913439783114 0.06951125075319986 0.004242871155051287 0.009999999999999787 0.03403028701627342 0.1814946250328884 0.1156224753340638 0.01000001919818461 0.04236568124536078 0.08328885345615689 0.02773647789558753 0.009999999999999787 0.0224486637674004 0.1086365113300047 0.1088491099993556 0.009999999999999787 0.0396186304209678 -0.2349225872001243 0.02536610325351851 0.002349850879480697 -0.1276053402025907 0.2963139259490389 -0.08035202643728523 0.3348450450788409 0.01000003467951227 0.07720944302784327 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2960023577426174 0.01000000000000001 0.5608624738376243 0.08433141796814481 0.0574358325558969 0.01000000000000001 0.1819794431063346 0.01000001585989307 0.1178491564064673 0.01000000000000001 0.09872939503155642 0.01000000000000001 0.2321286850172461 1.830908074126437 -1.795015951454465 0.2820162811254556 0.1803898206293589 -0.2086048064906058 4.882417034857144 -0.2346882073307602 12.94270407298889 1.333201022383744 -0.3207888714728488 -0.2539475546213841 -0.08152977844004512 -0.05020054885855487 0.9061935158740732 -0.0167788815466647 -0.8375973824605302 3.470774187214851 +0.07661236465552351 -0.2885080307165162 -0.02223617149113721 0.07502908513957962 0.1943414811644919 0.9212244980945816 0.2485382547932362 -0.00553900605707236 -0.1038164927383538 0.6647849924782929 0.4873693099970811 0.772161639177575 0.772161639177575 1.513686940890826 1.513686940890826 -0.3170961003669843 -0.3162248004157151 0.3149197635250567 -0.01075099785637068 2.38863067320618 2.300558047281123 -0.1489686451251688 -0.001805342398076171 1.029378393289614 -0.1260272950173826 1.616960025308663 2.490799741808574 -0.5804357926939554 -1.428424740112752 -0.3869742768626363 -1.701014594343264 -3.507582741375176 3.625981577623779 2.462369266326282 2.46236926632628 5.910398832921574 5.910398832921594 5.002870273388174 -1.351963356434169 5.990986819194079 0.8118650451215559 -2.896810215673629 -2.389327780649503 -2.275906512185497 0.08326933708138018 -0.01068322182481385 0.02831538238709808 0.01243627359933663 0.00525719272897307 0.150726207886601 -0.05167895662504041 0.1872232330736621 0.01000002923550403 0.03016426554644669 0.06784283320554785 0.0724017203737839 0.01940095938963182 0.03822339279243847 0.01000000022835179 0.01629755756928031 0.009999999999999787 0.02097781929076614 0.01059905848871123 0.01908507892412281 0.2573977465754695 0.2422461099183164 0.01032962564422357 0.005986466166302051 0.4008120228327137 0.3818539257601445 0.07422949955932578 0.03524826262520797 0.06703554882111984 0.002698263794196798 0.009999999999999787 0.03417892234293562 0.1877881025027426 0.1198963721873341 0.01000001533504369 0.04179328341376198 0.1052778549380835 0.03428601860729596 0.009999999999999787 0.02235854103015988 0.1044852984295646 0.1046593563907656 0.009999999999999787 0.04206698626688876 -0.2693533346611771 0.006454531788746998 0.00745898302988679 -0.1758663755378503 0.3249440413696325 -0.05881609583576708 0.3619876765448155 0.01000004060792192 0.09271077718230636 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.3420852349358666 0.01000000000000001 0.5695589935515469 0.1115964393706866 0.07000234856229703 0.01000000000000001 0.2101829379876756 0.01000000828546632 0.1437811176768748 0.01000000000000001 0.09979540351020011 0.01000000000000001 0.01778383973392338 2.265499502000762 -0.918435326529973 0.1457791216646372 0.1208663656578314 -0.2013983438195694 5.193345771946677 -0.1306568571333695 9.645104855788809 1.811936106004012 -0.1322823407948085 0.1569220878528954 1.249309862806559 -0.09860826332863036 0.9653381632774651 -0.009305978204525479 -0.3957168706854435 -1.359375170722483 +0.08357712507875292 -0.2803160930219124 -0.02251123085342055 0.08623133496987689 0.2117465812615373 0.9176562906001862 0.2373721735576657 -0.008677242073718894 -0.1155472967347166 0.636821447637415 0.5107272435958041 0.7851300915499007 0.7851300915499007 1.551202843479013 1.551202843479013 -0.2822742685190942 -0.3259050862515975 0.3559851228756989 -0.005707025444091851 2.368397218840639 2.285069113588113 -0.1636471910084754 -0.001441836869132906 1.329174780875232 0.05349810655551446 1.573422919040889 2.507050053805322 -0.4389759414397814 -1.788595792986571 -0.5068770889756078 -1.638605274014725 -4.482939128578321 3.068029507477147 1.323741811967257 1.323741811967257 4.856751488738461 4.856751488738461 4.981233069441965 -1.42438575246978 5.742717691509849 0.648998417584342 -2.893159440423402 -2.052756864531426 -1.918548819229998 -0.005399230545094724 -0.06056305953179253 0.02260352644506547 0.01201622305384342 -0.03192777604413077 0.1848297226774998 -0.05095247426183747 0.2212862472156809 0.01000003675423455 0.03073614296619187 0.08329605256226635 0.08891347741398947 0.01468578425341693 0.03343517661765105 0.009999999999999787 0.01750637402282162 0.009999999999999787 0.02193634853156601 0.01029673996234282 0.01772828201089638 0.2978996712834014 0.2785401256452911 0.01016325302489207 0.005306500101342593 0.4581591490960637 0.4353216170731229 0.09813838176872869 0.04920227644451058 0.07174439356652851 0.002267606078397044 0.009999999999999787 0.03427007694148987 0.2029024637782495 0.1321957927261659 0.01000000950504898 0.04120887126056161 0.1283515445201551 0.04105455266057811 0.009999999999999787 0.0223161773013052 0.1026829766110851 0.1029165014345823 0.009999999999999787 0.04030053888043383 -0.3037840821222297 -0.01245703967602479 0.01256811518029299 -0.22412741087311 0.3535741567902264 -0.03728016523424899 0.3891303080107901 0.01000004653633135 0.1082121113367694 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.3881681121291157 0.01000000000000001 0.5782555132654695 0.1388614607732287 0.08256886456869705 0.01000000000000001 0.2383864328690166 0.01000000071103979 0.1697130789472824 0.01000000000000001 0.1008614119888438 0.01000000000000001 0.2150359178168471 2.431200077433092 -0.5394871583468921 0.2397050155600552 0.1729931876246815 -0.1870248321223218 5.173753867578421 -0.07219746565139898 5.58090617397063 2.175982514751699 -0.0009071878811947382 -0.2704537155589 2.223078907765205 -0.05365058040544706 0.9690796248252403 -0.003064408836870934 -0.1349461773116584 2.260451656768041 +0.09054188550198235 -0.270028365809774 -0.02152408864078836 0.09650010230431327 0.229245743234527 0.9151580660741727 0.2236063707211504 -0.01234531467712285 -0.1261847480402163 0.6035055160632736 0.5301193760663319 0.7919501802373015 0.7919501802373015 1.581418984634627 1.581418984634627 -0.2479373224180108 -0.335967646369594 0.3928220419186044 -0.001507734235224767 2.348718039061747 2.272029215747498 -0.1757925895915164 -0.001146120338661127 1.614422195338915 0.2248665696144276 1.369503091095576 2.518112643354752 -0.2778455650640264 -2.152098205630347 -0.5398929496619007 -1.408703741747484 -5.06048575344086 2.515273206467019 0.6430680287622117 0.643068028762221 3.827986072972452 3.827986072972456 4.861167747724506 -1.459621495308875 4.800509146580612 0.5596026443421409 -2.728785894505023 -1.688406369819454 -1.586606189354575 0.06603263619590383 -0.09658822473640472 0.01554380605020267 0.01472956213242949 -0.06080284985176343 0.2158958505746411 -0.0383522323805936 0.2458634440068601 0.01000003760713142 0.03124673056499283 0.09913630717328381 0.1051232272821974 0.01227550657807708 0.03066903325565251 0.009999999999999787 0.01857310132366852 0.009999999999999787 0.0229992444637066 0.01014313665782662 0.01649560216100188 0.3359815596581956 0.3122655345798329 0.01007873397804548 0.005440210085021935 0.491522863108111 0.4651295849490311 0.1179092723308131 0.06416902328679619 0.07730031209065524 0.002456035245292476 0.009999999999999787 0.03420621426987358 0.2271925924191893 0.1514731933374098 0.01000000528076184 0.04081772198226119 0.1299364066065394 0.04541062378922778 0.009999999999999787 0.02233825802832534 0.1034048063169282 0.1027075431472446 0.009999999999999787 0.04188234918553757 -0.2190832320501863 -0.02032563315480473 0.04068215364430938 -0.1615728535702072 0.3601240616327535 0.07170794065834452 0.3276313094926419 0.01000003158663776 0.1265853032033357 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.4177225310028938 0.01000000368429654 0.5439564683199939 0.145669423187747 0.08458959531551258 0.01000000000000001 0.2880480801780049 0.01000000188088124 0.09875233850249721 0.01000000000000001 0.1072451523961209 0.01000000000000001 0.0007286252822922436 2.218421867913769 -0.2940070815174815 0.1083075493412401 0.1388241555330612 -0.1652532284549581 4.47294140047851 0.08649867406782158 2.938904946623318 2.098467115907983 0.04962279017557561 0.123668729742251 3.288320125032419 -0.05798394395277187 0.1711921170690778 0.007517699114675609 0.08126553087577454 -0.8953469907455814 +0.09750664592521174 -0.2579759788026328 -0.01945002957528441 0.1052263366966781 0.2468241444758239 0.9137939529954107 0.2075657088824623 -0.01610640683191589 -0.1350676141105849 0.5666596962986095 0.5459679958245367 0.7942040849902048 0.7942040849902048 1.60463452357201 1.60463452357201 -0.2148083880816802 -0.3461595129058948 0.4223683369648685 0.002126040108168414 2.330792253212918 2.261597401962224 -0.1859913904290833 -0.0008603057205620601 1.83591433482002 0.3656170559058043 1.130469292979756 2.52980439523164 -0.1133045964556416 -2.441844107084056 -0.5335885752989107 -1.13475872984097 -5.496581515728356 2.050474703829336 0.0125168287438262 0.0125168287438262 2.846459161976671 2.846459161976671 4.634340515690808 -1.461517372348176 3.649142735990623 0.4866046434786857 -2.389654991312843 -1.303819673056414 -1.359563979212222 -0.008242662844856863 -0.1107637078634482 0.008337691196098174 0.02204006450590601 -0.07313046810229196 0.2425248927424621 -0.008307165702353458 0.2548620805533561 0.01000003008519146 0.031709438672884 0.1157630378947792 0.1197430374191466 0.01112835546393454 0.02879419690497631 0.009999999999999787 0.01959578536612439 0.009999999999999787 0.02396198495326818 0.01007087991222111 0.01545003853137006 0.3700006952351265 0.3403182632241868 0.01003899168533895 0.006175754231027675 0.5023449862006588 0.4757041699055211 0.1314395991622712 0.07810696575116838 0.08114447043837636 0.002883696098911592 0.009999999999999787 0.03420352925697401 0.2606717409521409 0.1776620617359406 0.01000000391517908 0.04041076438328428 0.09886427847752133 0.04189833131206511 0.009999999999999787 0.02239460453949604 0.1066933618600796 0.1041365788067501 0.009999999999999787 0.04051679699504884 -0.1343823819781429 -0.02819422663358456 0.06879619210832577 -0.09901829626730452 0.3666739664752806 0.1806960465509382 0.2661323109744939 0.01000001663694405 0.1449584950699019 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.4472769498766717 0.01000000758842801 0.5096574233745182 0.1524773856022652 0.08661032606232832 0.01000000000000001 0.337709727486993 0.01000000305072279 0.0277915980577117 0.01000000000000001 0.1136288928033979 0.01000000000000001 0.2012868473371848 1.974633121959589 -0.2835395141665034 0.2270575576552145 0.1442325594150863 -0.1332925545140115 3.544787486779695 0.1006251802847226 0.05784960034069165 1.880551138664065 0.06779056257010911 -0.252885944409373 4.207775399522416 -0.05819009640068484 -1.290398986710441 0.006775350773232384 0.3354200226035808 1.414087298293807 +0.1044714063484412 -0.2450559932261385 -0.01655109149377054 0.112420085207507 0.2644848453560753 0.9135200355765045 0.1903176239500364 -0.01981657236255385 -0.1421372895171191 0.5271837699533033 0.5595194599769759 0.791539758005066 0.791539758005066 1.621299473270445 1.621299473270445 -0.1836981251640286 -0.3563103920424346 0.4442305921642631 0.005200506556109374 2.315870514124999 2.253875031912967 -0.1949378086558 -0.0006072334029489923 1.808550328607849 0.4554232272212904 0.9422032977144794 2.540664541261473 0.02720384407723309 -2.433420752700367 -0.5327267582120783 -0.9004999986663556 -5.772517502930894 1.92643632889627 -0.7823994113573236 -0.7823994113572876 1.957905421935468 1.957905421935488 4.279275143363675 -1.457387268364927 2.652816254080037 0.3911243565456801 -1.877304289104876 -0.9133628572044885 -1.220478620673593 0.05864341502776593 -0.09439005369821407 0.003278040547824546 0.032404235311426 -0.07392263723267245 0.2562989345369093 0.02646852336731298 0.2553649380007164 0.01000002257825461 0.03259164126348679 0.1316612426473505 0.133121451034151 0.010544985220589 0.02732916568794241 0.009999999999999787 0.02065498543504773 0.02648116587737004 0.0373777347196862 0.01003417528226302 0.01479661851123248 0.3976765605978794 0.3620737154363161 0.01001880409900613 0.006987729724143854 0.4893225358724562 0.4636644240507533 0.1389901013317667 0.08990061424068685 0.08251146943099474 0.003354541989034843 0.009999999999999787 0.03433716846352564 0.2874302328091556 0.2020382093418682 0.01000000336233153 0.03970016823249667 0.06400358327081079 0.03225804716564751 0.009999999999999787 0.02241157754732326 0.108315036132387 0.1066019893774799 0.009999999999999787 0.04146355039802518 0.08759182607337601 -0.01169292185246035 0.08950225561426384 -0.05727949786745307 0.2738040422766576 0.1878159628268088 0.2496607463854208 0.01000001471064538 0.1585604274616437 0.01000000000000001 0.01000000000000001 0.06958098104533494 0.01000000000000001 0.453710251701934 0.01000000416108726 0.4188500823499994 0.1460165271359846 0.08196365855362098 0.01000000000000001 0.3279433633199901 0.0100000026774264 0.0188957961860281 0.01000000000000001 0.1075013031666516 0.01000000000000001 0.09284307316962104 1.877371781732395 -0.1557964506716942 0.1063340615893928 3.353208193044596 -0.0500024903044938 2.714151099367891 0.1272902505709095 -3.474856935721551 1.470563050133629 0.06451525908263527 0.1723758928002873 2.561124115131864 -0.1541601938524639 -1.355698933514051 -0.002595055924146938 0.3361741061745209 -0.573260093618131 +0.1114361667716706 -0.2336980351023161 -0.01326525682437607 0.1184471191903889 0.2822007833983244 0.9140692169588744 0.1747515600143097 -0.02353963486033805 -0.1476826248747418 0.4871818490867437 0.573992939353845 0.7832388485365609 0.7832388485365609 1.632169905794588 1.632169905794588 -0.1554787922952143 -0.3665156457376546 0.4596550446058911 0.007502693598239851 2.304892562087156 2.248879884924076 -0.2031451570920355 -0.0003536619906583383 1.387362666756636 0.4767182465746131 0.7954356501691704 2.54566629388607 0.1230563840381 -1.958835103306525 -0.5372920672666728 -0.6970293734004986 -5.64762605686716 2.315240515864495 -1.606083709049234 -1.606083709049234 1.182506112005804 1.182506112005804 3.804181293210283 -1.477131565914832 1.80046385355921 0.2648369561791153 -1.257157443533261 -0.5206705352134549 -1.147332762193626 -0.008099342860056868 -0.04080563988556385 0.002120614456324166 0.04464014078073797 -0.06701730954004326 0.2507719437646196 0.05623891310867979 0.2527956678365455 0.01000001802886707 0.03357097669285025 0.1462383462413053 0.1460368549386573 0.01026994537031634 0.02636441238478104 0.009999999999999787 0.02148415313062735 0.06385554408404026 0.06572524318795381 0.01001691939720306 0.01481399573299402 0.4176496920987969 0.3782872942032443 0.01000931243409475 0.007875697202753162 0.4506082115898193 0.4278627862942539 0.1404040570505178 0.09809602540903617 0.08105203124096461 0.003741935885562597 0.009999999999999787 0.03494861958047091 0.3001147988433566 0.2101202299526244 0.01000000290464031 0.03814783355762152 0.04006294599837812 0.02471635519840021 0.009999999999999787 0.02234879412687985 0.106428044736504 0.1083126651607271 0.009999999999999787 0.04045686503739176 0.3095660341248949 0.004808382928664034 0.110208319120202 -0.01554069946760156 0.1809341180780344 0.1949358791026794 0.2331891817963481 0.01000001278434659 0.1721623598533856 0.01000000000000001 0.01000000000000001 0.1291619675707917 0.01000000000000001 0.4601435535271963 0.01000000073374641 0.3280427413254808 0.1395556686697039 0.07731699104491374 0.01000000000000001 0.318176999152987 0.01000000230413023 0.01000000000000001 0.01000000000000001 0.1013737135299054 0.01000000000000001 0.2291795048927322 1.841675293649168 -0.1398797517423624 0.161001481545743 4.432035095094802 0.05933352338387953 1.953383588151487 0.1224470322597872 -6.765613220436475 0.8472816212112628 0.0438264953142809 -0.1156775176251029 -0.4712446976441581 -0.2999036580925717 -0.6877674382999053 -0.0161274982626541 0.118690446411803 0.8531379924421912 +0.1184009271949 -0.2266396048068349 -0.01008003487171427 0.1235322089295128 0.2999296784975996 0.9151146848252738 0.1641119595374709 -0.02728922851914106 -0.1518502878887853 0.4496768329530947 0.5928952840016866 0.769473666196177 0.769473666196177 1.638020982523129 1.638020982523129 -0.1308909195730799 -0.3769523911469177 0.4693987012716323 0.008935830021712832 2.298412892059454 2.24662373905451 -0.2110042025976009 -0.0001392148584300479 0.6204826902533753 0.4318265823784655 0.6651571163747323 2.545385751962232 0.1726185416819281 -1.071780931715714 -0.5379604005088543 -0.4987043409570768 -5.08653713496871 3.129792861613153 -2.319474877730261 -2.319474877730223 0.5025432070443143 0.5025432070443365 3.253142571853113 -1.52133172374081 1.002767447716943 0.1509703693305426 -0.6098212896160891 -0.1279959955923355 -1.118583264083785 0.04942068003125977 0.03440296997553371 0.002074171811113246 0.05270152529805783 -0.06160920133710768 0.2314212677106644 0.07373281395776132 0.2456838567842157 0.01000001508704873 0.03468254194428599 0.1594966053619373 0.1586417646171485 0.01013020547790777 0.02588705911671951 0.009999999999999787 0.0220485864682618 0.09736364489747551 0.09394595034780107 0.0100081541731285 0.01560505042473537 0.4314071320506829 0.3893156230124757 0.01000448971142553 0.008836804203824045 0.3946209592911929 0.373654946410837 0.1399421438965138 0.1032648063411017 0.08435037605363194 0.004155334397996313 0.009999999999999787 0.03587518415069058 0.2899965918813008 0.1946125302585533 0.01000000228657827 0.0356272881826909 0.02513446932830776 0.02068767399363747 0.009999999999999787 0.02222616094992125 0.1040375408221861 0.108619750997887 0.009999999999999787 0.04094997988894411 0.4383079291222383 -0.0009093039562133587 0.06962248229817136 -0.05730808068890186 0.1083781468716315 0.1138340638864507 0.1947571482685876 0.01000001198246314 0.182707466499086 0.01000000000000001 0.01000000000000001 0.141950641402704 0.01000000000000001 0.4575752650783247 0.01000000000000001 0.2413875967483772 0.1391156851496765 0.097519785481882 0.01000000000000001 0.2313877619408194 0.01000000121435529 0.01000000000000001 0.01000000000000001 0.1008010232073284 0.01000000000000001 0.1224878910630641 1.765889539944698 -0.01920261431610482 0.03174706452414285 3.573879365825717 0.1628856028631135 1.216902301958376 0.1497159538836648 -8.639112394101263 0.6664206261768523 0.07936743193936761 0.2647958964574135 -3.835427323325296 -0.4110007880123216 -0.4343144286457878 -0.01848187035689043 -0.01150468875769439 -0.3663781339976848 +0.1253656876181294 -0.225320405958799 -0.00733541311891317 0.1277174375423855 0.317657122498515 0.9164104491096445 0.1601655378533273 -0.03101254250370467 -0.1546146830330928 0.4168271386135509 0.6178267816145611 0.7513092540669133 0.7513092540669133 1.639237714576343 1.639237714576343 -0.1102104260140551 -0.3877272429614145 0.4736961568589142 0.009665089620094935 2.296310604648589 2.24708593155042 -0.218853428489755 5.254230068540977e-05 -0.2607145387049035 0.3501955313338612 0.5370316070786219 2.545249596605305 0.194931481744919 -0.03682246079165852 -0.5297455982856492 -0.2940670934465666 -4.310804253576841 4.046571559220338 -2.869366154234804 -2.869366154234804 -0.148291903627678 -0.148291903627678 2.682138631100443 -1.574222171967457 0.2365332234941353 0.06271174805222435 -0.0001486303553179802 0.2599272744174992 -1.144512962868311 -0.01461499357551688 0.119300338785759 0.001001315071581743 0.0519584585307733 -0.0647391819918095 0.2024411942036828 0.07338230036597793 0.2329035334221259 0.01000001327932321 0.03572947164213591 0.1714073529914888 0.1704668609396709 0.01006447495674578 0.02579040110617736 0.009999999999999787 0.02235353606986346 0.119945901451425 0.11414339405568 0.0100040339116636 0.01701410131817793 0.4395894495428658 0.3952854083428519 0.01000222137957518 0.009907840688243574 0.3258091902226772 0.3097750030610262 0.1394894513750518 0.1077889686953508 0.09545830570277047 0.004909924511307562 0.009999999999999787 0.03700798882562228 0.2508355855124322 0.1587354861807597 0.01000000141852375 0.03260241271109532 0.01757966085821971 0.01915122537253744 0.009999999999999787 0.02209979131456841 0.1024660990254023 0.108417113255677 0.009999999999999787 0.0401612653732375 0.5670498241195817 -0.006626990841090974 0.02903664547614071 -0.09907546191020217 0.03582217566522838 0.03273224867022184 0.1563251147408272 0.01000001118057947 0.1932525731447864 0.01000000000000001 0.01000000396628298 0.1547393152346164 0.01000000000000001 0.455006976629453 0.01000000000000001 0.1547324521712736 0.1386757016296493 0.1177225799188503 0.01000000000000001 0.1445985247286519 0.01000000012458024 0.01000000000000001 0.01000000000000001 0.1002283328847514 0.01000000000000001 0.2106187649160149 1.617744157886995 -0.03055933456308502 0.08648815464944734 2.128075457409157 0.236797259146758 0.5007726807548479 0.1540143731781347 -9.543053614042565 0.6621730061193211 0.1418030458833746 -0.05645362704258676 -6.32050386704676 -0.4447270449749058 0.02790185460128584 -0.01720045933143523 -0.02768173254792932 0.4850479825581987 +0.1323304480413588 -0.2300165120146374 -0.005215450154825163 0.1310031867194539 0.3353941175732618 0.9178112523722919 0.1633451539910178 -0.0346420392161666 -0.1559310493987605 0.389598635293658 0.6487848119966899 0.7299688918090554 0.7299688918090554 1.635897595291246 1.635897595291246 -0.09346228374086873 -0.3988758876847567 0.4726539873134832 0.009937095780199634 2.298194596855816 2.250230592680228 -0.2271254490563499 0.0001855459568540496 -1.054596220142535 0.2604658713427597 0.4037485756721209 2.548764641192221 0.2080539060160982 0.9131649733668268 -0.5093137763210056 -0.08156122952999922 -3.522221373062178 4.791422867953359 -3.218696804531435 -3.218696804531453 -0.8173239826232239 -0.8173239826232235 2.134415236274453 -1.622924441868073 -0.541679638399847 0.01898427909765354 0.5292823525355277 0.6364819685421845 -1.250620073459187 0.03497485969591319 0.1880687942589483 -0.002369950873770676 0.04347428685625321 -0.07436130322486534 0.173368182357029 0.06342834325714142 0.2149530233921935 0.01000001148833318 0.03677858396719813 0.1812982139622044 0.1808801348939877 0.01003108586169787 0.02589217915872366 0.01000000108062382 0.02256042350131349 0.1320792917030147 0.1241457543868774 0.01000194102495477 0.01875155834882936 0.4422393201439783 0.3962396944143229 0.01000106835447845 0.01104771152185213 0.2593012801136405 0.2449200114479648 0.1398201827635952 0.1128052880914767 0.1060250172546424 0.005974319167046271 0.009999999999999787 0.038055473970251 0.1969759426901998 0.1150549765141107 0.0100000004105909 0.02949548634764199 0.01369683367614005 0.01933690466282467 0.009999999999999787 0.0220086838556286 0.1016423713903958 0.1082044292921918 0.009999999999999787 0.0402410194313978 0.4374664788978292 -0.02807947332261063 -0.01772041593352253 -0.1359121770928657 0.04658447262620513 0.004584001521107939 0.1119161992015748 0.01000000868170925 0.1976670085156859 0.01000000000000001 0.01000000410690172 0.1418070327466556 0.01000000000000001 0.4409472490192435 0.01000000000000001 0.1314968711989686 0.1416402728238868 0.1200412317013608 0.01000000000000001 0.08256399481172938 0.01000000000000001 0.01000000000000001 0.01000000201249829 0.1009684412364119 0.01000000000000001 0.1150066556597353 1.348187887582887 0.04085837953030817 -0.002984590316380291 0.7822959043660077 0.2501499313325242 -0.2320149642587001 0.1720000075233968 -8.869411789653215 0.7920835376139373 0.1617448669021043 0.2510085779647563 -6.11199065906832 -0.442137075247794 0.0502378265272528 -0.009591522389921314 -0.0273986277340757 -0.2563405272656879 +0.1392952084645882 -0.2395476504031331 -0.003680879451803065 0.1333030822033048 0.3531693899193433 0.9193187206397551 0.1723740551325847 -0.0380626517379028 -0.1557176489728342 0.3675680020930727 0.6838459124226337 0.707031998961376 0.707031998961376 1.627762693645234 1.627762693645234 -0.08037932607440057 -0.4102739443197025 0.4660689538780787 0.009979493404732143 2.303517864596981 2.255859727117281 -0.2365489401669105 0.0002913138928266612 -1.649142004649775 0.1820943403393875 0.253933945907534 2.556241294526754 0.2255613952901174 1.642857714388098 -0.4697624321661706 0.1452206059513212 -2.818184320241524 5.224808797687986 -3.327825740157118 -3.327825740157118 -1.52516119117394 -1.52516119117394 1.629644536832503 -1.645843274906208 -1.355155091140538 -0.003222619166271645 0.9874734341344786 0.9733684141738519 -1.47516914456085 -0.02243600062842477 0.2209384107367596 -0.009013658405292446 0.02806326953834581 -0.08890887672551973 0.1515047916588754 0.05018147987527666 0.1922139203867723 0.01000000932219036 0.03767081603344158 0.1888353601162196 0.1889073886643642 0.01001538933566248 0.0260958873069268 0.01000000265053735 0.02264757707307563 0.13307299260891 0.1255710383817785 0.01000095731702944 0.0203316829196738 0.4393031719954124 0.3919800729904512 0.01000052586889666 0.01228545915646873 0.2056613164407501 0.189171492413764 0.1412895926952951 0.1190141321039029 0.1127205272181584 0.007133651362632776 0.009999999999999787 0.03902453471097367 0.1362365730407666 0.07514173187822992 0.009999999999999787 0.02651776830768826 0.01183488569511049 0.0201967453804559 0.0100000011550625 0.02195741637144932 0.1015307586286278 0.1081189563274947 0.009999999999999787 0.039457333689636 0.3078831336760767 -0.04953195580413039 -0.06447747734318582 -0.1727488922755293 0.05734676958718177 -0.02356424562800619 0.06750728366232261 0.01000000618283903 0.2020814438865851 0.01000000000000001 0.01000000424752046 0.1288747502586951 0.01000000000000001 0.4268875214090341 0.01000000000000001 0.1082612902266635 0.1446048440181245 0.1223598834838716 0.01000000000000001 0.020529464894807 0.01000000000000001 0.01000000000000001 0.01000000780201382 0.1017085495880723 0.01000000000000001 0.1655690433760564 0.9325748085686846 -0.001289097125493912 0.05210539012963428 -0.334917040800818 0.1916176836119659 -0.9964525441015433 0.1821206668789789 -6.928039736973821 1.004618271777237 0.1690653015906944 -0.07897397209880386 -5.238696367025891 -0.407623593986302 0.2214939916956152 -0.00576001563376821 0.008848166881042523 0.2371026845165614 +0.1462599688878176 -0.252318306675114 -0.002617680874174688 0.1344675047696682 0.3710118521872352 0.9209938986921267 0.1854655305351334 -0.04111566150914836 -0.1538516593016408 0.3499679443438843 0.7205923938837326 0.6842307621628576 0.6842307621628576 1.614534651879096 1.614534651879096 -0.07063392968953863 -0.4216893419945582 0.4535499168338157 0.009940714524066863 2.311818465496925 2.263607442378942 -0.2480200651538906 0.0003532571108708638 -1.975966679996157 0.1278436910940259 0.07887189099505498 2.568498296544746 0.2583505288055399 2.068517542529763 -0.4040923608494444 0.3904028659597452 -2.270753093753647 5.267448575409545 -3.201067394274592 -3.201067394274578 -2.28005860706262 -2.280058607062613 1.176688776311028 -1.627110188837639 -2.241866535110554 -0.008683148173378008 1.384058453230603 1.240376621542037 -1.829902312514421 0.02428143103999592 0.2254697495009097 -0.01744288032041297 0.008624364015099584 -0.1029815943253882 0.1333725368298229 0.03505567684068467 0.1695726246605442 0.01000000742638241 0.03843949023558579 0.1932541649383128 0.1934327711798285 0.01000741691311102 0.02626381667100741 0.01000000219188735 0.02274778005878053 0.1284907004203855 0.1211333573878086 0.01000045787948212 0.02133638662087822 0.4303057599402655 0.382153464183598 0.01000024994831916 0.01364988440141701 0.1658268728984025 0.1482039843054594 0.1358245240027109 0.1222366429783239 0.1124729147528636 0.008277154596642688 0.009999999999999787 0.03975260722994323 0.08425655468715298 0.04480964824729483 0.009999999999999787 0.02386050430480058 0.01088719633015689 0.02138065297695446 0.01000000256505285 0.02193316158988168 0.1019481465076129 0.1082993826628078 0.009999999999999787 0.03912334943125106 0.1878258137610053 -0.06153547891938133 -0.09390615958539189 -0.1613491501103044 0.04511266015299698 -0.0430892490616222 0.06608079378112441 0.01000000522430655 0.1978562025819193 0.01000000000000001 0.01000000000000001 0.1168869629391429 0.01000000000000001 0.3990265570401165 0.01000000000000001 0.1050712554829998 0.1145007008010368 0.1033357148202081 0.01000000000000001 0.01611645649638482 0.01000000000000001 0.01000000000000001 0.01000000059066708 0.1031094804248842 0.01000000000000001 0.07736580797551172 0.3567748372187224 0.03259334877773952 -0.002619242989225075 -0.8754023381300301 0.1000014481734388 -1.824906519700897 0.2063659073277362 -4.866556982926801 -0.1579114565919444 0.1560889488597666 0.1902128908914781 -3.457909951112464 -0.3537964054684066 0.1406995543270327 -0.00209865487656234 0.04613555743575957 -0.2123266767292392 +0.153224729311047 -0.2664853213693368 -0.001835584132108714 0.1343798654306938 0.3889626583801427 0.9229573687415216 0.2005192907624003 -0.04365180417343639 -0.1502825044798213 0.3354512707379795 0.7563851391213054 0.6627032581348931 0.6627032581348931 1.595909848306922 1.595909848306922 -0.0638793223868741 -0.4328678924177289 0.4348121876912376 0.009847815200459298 2.322629034937586 2.27298312570184 -0.2621921643881726 0.0004074756818934766 -2.050120168247272 0.1013731615844069 -0.1056080424039201 2.587353326652369 0.3083473001912127 2.206347254710415 -0.3213389184923194 0.6343013631141181 -1.93275999668986 4.950933986310071 -2.962060748078342 -2.962060748078342 -3.074894844054552 -3.074894844054552 0.7708098029883885 -1.577830238618656 -3.140914175565011 -0.01876384085759675 1.708235782933599 1.44085558165887 -2.250780675519669 -0.02465420959802023 0.2074523229759677 -0.0265189996312003 -0.01262254821521358 -0.112444161028153 0.1163077619593693 0.01913368145361982 0.1507709053668878 0.01000000602729845 0.03905775393609145 0.1941430768475616 0.1937355839015509 0.01000367039582795 0.02631423280785983 0.009999999999999787 0.02289959865035396 0.1206046582446358 0.1142686582200625 0.01000022353218899 0.02176795514733643 0.4148977127970488 0.3665662549141242 0.01000011994919214 0.01511376821880672 0.1397857151766742 0.1209596370169361 0.1194504777145999 0.1157186779111146 0.1037310477840805 0.00926312560376541 0.009999999999999787 0.04031132390217751 0.05125099633857388 0.02716347865279367 0.009999999999999787 0.02161239415850424 0.01043954522301815 0.02246617491935554 0.009999999999999787 0.02191573472622466 0.1028131517989044 0.1088057941292186 0.009999999999999787 0.03818077977106826 0.06776849384593353 -0.07353900203463226 -0.1233348418275979 -0.1499494079450797 0.03287855071881196 -0.06261425249523822 0.06465430389992632 0.0100000042657743 0.1936309612772533 0.01000000000000001 0.01000000000000001 0.1048991756195908 0.01000000000000001 0.3711655926711989 0.01000000000000001 0.1018812207393363 0.08439655758394926 0.08431154615654446 0.01000000000000001 0.01170344809796253 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.104510411261696 0.01000000000000001 0.122377176747721 -0.2799779677257517 -0.03503418528251152 0.06692732327537264 -1.03185936943502 0.02703623646280391 -2.650666074084973 0.2106811819300233 -2.987298064315094 -1.792455956130928 0.1238283609743181 -0.1276061824007533 -1.595810390742169 -0.2901310079572139 0.1932415757841438 -0.003799311311215804 0.1024580289053327 0.06234135331718778 +0.1601894897342764 -0.2803853407748749 -0.001140323998892523 0.1330369323883041 0.4070722014702777 0.9253248830723297 0.2156408982855735 -0.0455901371576517 -0.1450885007755125 0.3224476473903781 0.7888804661167184 0.6428752452649631 0.6428752452649631 1.571647323591336 1.571647323591336 -0.05981017432259694 -0.4436439695750058 0.4098889784346156 0.009653715592983669 2.335415930002691 2.283559031667743 -0.2794090028924563 0.0004413167373713556 -1.930645180468213 0.09961370358215804 -0.2748648302438292 2.613673868166369 0.3716507100379092 2.124024292186003 -0.2366812363215578 0.8494683822972009 -1.820111150532791 4.365987372107586 -2.741942292127518 -2.741942292127522 -3.889943758014135 -3.889943758014111 0.4022897776133005 -1.520616590384716 -3.998312026347062 -0.03991591492659952 1.946769174054432 1.592541910535222 -2.678340344976782 0.02019163929398182 0.1796196350298382 -0.0350457905574979 -0.03423487068054598 -0.1142160749561114 0.09770075970584946 0.003888146659279279 0.1362661682461082 0.01000000526539768 0.03966645794646562 0.1914501115262452 0.1896551322701354 0.01000176925447693 0.02616409639383654 0.009999999999999787 0.02323088035702625 0.1125478378577611 0.1073664952197562 0.01000010502016435 0.02177436662683707 0.3938033164588961 0.3454202329357741 0.01000005371562107 0.0166164255664758 0.1206349046954545 0.1037201985621317 0.1016983597238328 0.1041235345719791 0.08799538140888519 0.01004146881539913 0.009999999999999787 0.04061849978493726 0.04352062854845329 0.02296615618530851 0.009999999999999787 0.01978819048287228 0.01021206813352604 0.02351961964120708 0.009999999999999787 0.02187881929200275 0.1038673845545328 0.1096884231600166 0.009999999999999787 0.03741438696607213 0.03957909498267109 -0.0739693505720283 -0.1408625298031329 -0.09619454616284212 -0.005291406985265978 -0.06789133636057626 0.07582696254268884 0.01000000479913488 0.1820010833217157 0.01000000154802194 0.01000000000000001 0.09996021155480583 0.01000000000000001 0.3355087470320069 0.01000000000000001 0.08966450845913232 0.07489283318905238 0.05568390372207566 0.01000000000000001 0.05478948676587603 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.105738463495294 0.01000000000000001 0.06905656001228971 -0.8759184767399851 -0.0183565694158573 0.04068770461187544 -0.9368253181661944 -0.01892232980575645 -3.397610313434717 0.218207663191109 -2.067183220644137 -1.529172267261204 0.09383487187052612 0.1266852825486169 0.3489594864316515 -0.2380470780411568 0.1285557233991236 -0.007261079746755092 0.150696325251444 -0.2119459175775579 +0.1671542501575058 -0.2932283599455978 -0.0004294027761564401 0.130623057406436 0.425379527385326 0.9281362741903743 0.229939502061804 -0.04696828183117008 -0.1385576301573361 0.3098367910348916 0.8170001935417974 0.6243673380395838 0.6243673380395838 1.541758030001298 1.541758030001298 -0.05821149573757811 -0.4541049239801143 0.3793644537889502 0.009250823180068757 2.34951148421279 2.295116621183192 -0.2992931020496181 0.0004912098543692345 -1.746592072353594 0.1058703986950693 -0.4131396370020113 2.644148625527087 0.4358109199759141 1.970013570010641 -0.1604752917046213 1.018195950328633 -1.81997306125287 3.694437192607589 -2.582979325522654 -2.582979325522654 -4.690677306779136 -4.690677306779136 0.06139000126453587 -1.487339616393057 -4.749387417620884 -0.07872076478596624 2.084030229684187 1.722755744738518 -3.016711013619944 -0.02004470100290989 0.1517223875093139 -0.0421072412109238 -0.05510793272384706 -0.1059384366532146 0.07555314231527444 -0.009553241090801912 0.1264086725074547 0.01000000518994115 0.04025144291375415 0.1850961502213688 0.1817551821243315 0.01000087845956577 0.02591537261704158 0.009999999999999787 0.02364025782910328 0.105808973388581 0.1014049480250856 0.01000004999519399 0.02159175307377836 0.3673867823207666 0.3195736671131839 0.01000002243316889 0.01811688313033288 0.1041055496834908 0.09071601664177731 0.08730385581482114 0.09452992470954591 0.06574168714265216 0.01048877209986765 0.009999999999999787 0.04083445796153473 0.06064212341291242 0.03144557129675629 0.009999999999999787 0.01823605946539253 0.010105019156323 0.02452559784830655 0.009999999999999787 0.02180818715354471 0.1049989491820034 0.110900724462347 0.009999999999999787 0.0362101260176444 0.01138969611940877 -0.0743996991094244 -0.1583902177786677 -0.04243968438060464 -0.04346136468934403 -0.0731684202259143 0.08699962118545135 0.01000000533249545 0.1703712053661782 0.01000000685885849 0.01000000000000001 0.09502124749002105 0.01000000000000001 0.2998519013928151 0.01000000000000001 0.07744779617892861 0.06538910879415571 0.02705626128760674 0.01000000000000001 0.09787552543378986 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.106966515728892 0.01000000000000001 0.1155660183872486 -1.376784830412534 -0.06334452383811566 0.08935329670259375 -0.7617522162380375 -0.0272441755142315 -4.000473837339421 0.2100494922008759 -1.771102164742678 -1.217695776261553 0.02876696419385271 -0.1538002510199045 2.044440474657595 -0.2120026470056875 0.1796110955210799 -0.01348139718332428 0.1971266899254894 -0.06339682478612672 +0.1741190105807352 -0.30488177001493 0.0003081668661337744 0.1273705082816012 0.4439024551856496 0.931366213540989 0.2432795967389274 -0.04785628826677346 -0.1310341245358946 0.2971921284290668 0.8405823659502278 0.6067994262282044 0.6067994262282044 1.506428324351542 1.506428324351542 -0.05890630412659981 -0.46448660755315 0.3440088779060773 0.008596248371375736 2.36417891494877 2.307569345690879 -0.321112998594522 0.0005508525564454914 -1.612300104795705 0.1037975894076961 -0.5167105483709959 2.674441525355701 0.4893095689678137 1.875211445063958 -0.09538209786117058 1.135556384208932 -1.797782561581304 3.095547382240702 -2.461329891541766 -2.461329891541767 -5.446309784289179 -5.446309784289179 -0.2555330127764277 -1.49941468569871 -5.389467886766483 -0.1073979187718352 2.112896572443999 1.852186979996063 -3.227232267669252 0.02392336745304835 0.1291648820878444 -0.04731042946550001 -0.07402024137413399 -0.09094806077242268 0.05380828036531904 -0.01789565661734649 0.1174880862332532 0.01000000581432481 0.04086285362706477 0.1763163294560628 0.1707540490099051 0.01000043405486206 0.02567653850224705 0.009999999999999787 0.02406572344733293 0.09992391309082826 0.09642621766164927 0.0100000260983224 0.02145028809134208 0.3374288137955235 0.2901222492410067 0.01000000644369159 0.01960182063508764 0.08659754738627123 0.07814245625125338 0.0742609051373968 0.08555568223917387 0.04512298163817086 0.01069548524715636 0.009999999999999787 0.04095789000069505 0.08164454501254292 0.04439106443567109 0.009999999999999787 0.01676982120192605 0.01005063940241646 0.02571125506815708 0.009999999999999787 0.02171822547877955 0.1062925137583632 0.1123479633449027 0.009999999999999787 0.03505174873334793 0.04120149534861839 -0.06767297686314222 -0.1613006339290197 -0.004477514352742351 -0.04629645591149129 -0.0393970653739964 0.06755730074843447 0.01000000728628125 0.1574567964758351 0.01000003152083051 0.01000000000000001 0.09061184044436243 0.01000000996774797 0.2648636609293127 0.01000000000000001 0.05500448094452148 0.053605634444855 0.01896415757245262 0.01000000000000001 0.1118104119338111 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1087046628552196 0.01000000000000001 0.07217442528294282 -1.767971305701039 -0.01619682145733274 0.0436432146933242 -0.6786438735214273 -0.01219315472256796 -4.431790860657641 0.2164065319870597 -1.884798351484244 -1.351217851353005 0.03324621848104877 0.1045186792811587 1.711305961948648 -0.2105213441974368 0.1709884397951004 -0.01225936303550428 0.2171288046095064 -0.2282168445122864 +0.1810837710039647 -0.3158610461911406 0.0009867439512638576 0.123483356009439 0.4626269175446316 0.9349188497654382 0.2562620622754879 -0.04830885385099171 -0.1228331582747511 0.2849795894234415 0.8603722273824674 0.5900890288560201 0.5900890288560201 1.466008993919104 1.466008993919104 -0.06169603373612764 -0.4750683741241639 0.3044846557186713 0.007780576784472526 2.378734577502509 2.320903039510996 -0.3439424531193778 0.0006399080568897375 -1.553007842001013 0.08893010108788912 -0.5953722218322501 2.702021271938662 0.5284765263204028 1.867316816997436 -0.03543413452990851 1.212735488957567 -1.695884485622829 2.605437501111422 -2.336735553425182 -2.336735553425181 -6.15221351861573 -6.15221351861573 -0.5401873950014573 -1.544795432766924 -5.946455529433496 -0.1249813016730945 2.051934888151894 1.975734816205975 -3.306615595714773 -0.01159852437029141 0.1160854717949107 -0.05035447454723041 -0.09003643634945968 -0.07180011870540115 0.03548581645969762 -0.01861608203827902 0.1066632165628216 0.01000000714324578 0.0414262896458677 0.1657206617621605 0.1573277752685081 0.0100002382391704 0.02553712228659188 0.009999999999999787 0.02439890210435536 0.09466658511749415 0.09180265814038746 0.01000002198036221 0.0214384250868429 0.3049837795911521 0.2581892085273774 0.009999999999999787 0.0211319004889754 0.06626738309050184 0.06383097782370051 0.06155623398683829 0.07582134142983588 0.03012900038791422 0.01098883732323408 0.009999999999999787 0.04111015624572678 0.09872183059314299 0.05581701909424908 0.009999999999999787 0.01528299432308211 0.01002507189026902 0.02704843954142344 0.009999999999999787 0.02163871034365217 0.1077847961957774 0.1139066496853887 0.009999999999999787 0.03360264517630362 0.07101329457782801 -0.06094625461686004 -0.1642110500793718 0.03348465567511982 -0.04913154713363843 -0.005625710522078498 0.04811498031141737 0.01000000924006716 0.144542387585492 0.01000005618280253 0.01000000000000001 0.0862024333987037 0.01000002339195949 0.2298754204658103 0.01000000000000001 0.03256116571011447 0.04182216009555428 0.01087205385729839 0.01000000000000001 0.1257452984338325 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1104428099815472 0.01000000000000001 0.1017896092643909 -2.07318800285513 -0.03479568853358961 0.06285218107403141 -0.65976138805069 0.009972317247538807 -4.713096437586125 0.2230125652688093 -2.270162557745368 -1.4359655493274 0.05364619143528462 -0.1455202229766705 1.608089936129546 -0.2179148720148956 0.2231236484901741 -0.01048156976447363 0.2291299322583849 -0.1468817560653098 +0.1880485314271941 -0.3267304412875056 0.001510417176349144 0.1191092825950251 0.4815277893730441 0.9386867834182659 0.2695356815339127 -0.0483446035404711 -0.1142044198247443 0.2738806000175389 0.8771880744072407 0.5743732493439202 0.5743732493439202 1.420855404102328 1.420855404102328 -0.06633569668341499 -0.4860435433654051 0.261206084200527 0.006879235182793764 2.392611869587592 2.33505561448904 -0.3671056286956569 0.0007332753027928796 -1.570168473458937 0.06091415478414408 -0.6580808823736888 2.724976147297049 0.5515047045126114 1.945744284328083 0.02496701537565427 1.260808162500408 -1.486434744864753 2.230564247828019 -2.170133705654576 -2.170133705654578 -6.805055047673244 -6.805055047673251 -0.783498252719611 -1.604895120217358 -6.463533405516172 -0.1359583035021461 1.928548176760419 2.083147301491101 -3.31018960130197 0.02639676563200632 0.1061507307531029 -0.05267940082695688 -0.1032659013501642 -0.0527108620816783 0.02241338604407295 -0.01381499428697186 0.09262859685278713 0.01000001014102647 0.04194972065002966 0.1527289611182296 0.1419138323360718 0.0100001367288276 0.02552349689238032 0.009999999999999787 0.02462099707885557 0.08164564810200181 0.08551966694175617 0.01099897766605951 0.02223132593971311 0.2700375817949308 0.224648331747348 0.009999999999999787 0.02277136225229137 0.0472338116784643 0.04858638438803453 0.04985986009319276 0.06609777295730268 0.02031779508810239 0.01137720487457861 0.009999999999999787 0.04122839317246063 0.1136756334197835 0.06634139404510808 0.009999999999999787 0.01373018149159533 0.01001208487083183 0.02861352262718952 0.009999999999999787 0.02157092664494709 0.1095076638998638 0.1155509161512276 0.009999999999999787 0.0321425795535295 0.05165702347450207 -0.06539058740014575 -0.1625058033256999 0.03441066949222538 -0.02546323794058331 0.02079625395212992 0.0108220956677233 0.0100000160250211 0.1257708394515407 0.01000003051417275 0.01000000000000001 0.04899412029864658 0.01354882955563397 0.1912238475338263 0.01000000000000001 0.02128058036565983 0.03318667564050992 0.01043602822535683 0.01000000000000001 0.1378607825598014 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1127181821548718 0.01000000000000001 0.05866086162371456 -2.340636234019591 0.01709761809131047 0.01287986717719371 -1.307297370438737 0.2114631733170197 -4.894697787373262 0.2452161391204196 -2.070126093922088 -1.354672838424654 0.06814536852239501 0.09881767096158342 1.435233640527255 -0.2257709049013174 0.2292460588909244 -0.009810014383458012 0.2472550894729673 -0.2477354819718229 +0.1950132918504235 -0.3377595098817787 0.001827814725779753 0.1143528971523375 0.5005765159658249 0.9425729325012417 0.2833849849111614 -0.04796387848640649 -0.1053303806340931 0.2643420412196731 0.8915427176474284 0.5599446949155249 0.5599446949155249 1.371343654431327 1.371343654431327 -0.07248944401559454 -0.4973967974103273 0.2146998189036298 0.005857345498282207 2.405535383025449 2.349848266433066 -0.3895680317998993 0.0008402657805368641 -1.598859669832435 0.02969599593059247 -0.7051569107650459 2.744479233270988 0.5624232590046265 2.032630218705452 0.08416071676097436 1.283176028399513 -1.247791367347924 1.898678435462834 -1.967096109857306 -1.967096109857306 -7.403702324937466 -7.403702324937466 -0.974971735850569 -1.653366232250232 -6.873319795431849 -0.1595978786620322 1.778047894410823 2.159539262326975 -3.1053818333291 -0.007686478295972243 0.09451082752238005 -0.05538790937093152 -0.1137923971274217 -0.03690889262598951 0.01599050401239266 -0.005100837082070875 0.07438456384138803 0.01000001514587989 0.0423846815031137 0.1370303469805778 0.1248971490627193 0.01000007541199732 0.02558325984987331 0.009999999999999787 0.02474484373061436 0.05669928511882372 0.07132441098397369 0.01334119733244865 0.02429689263857426 0.2325483726540045 0.1903399508360919 0.009999999999999787 0.02451199825618255 0.03150127311243978 0.03551534573428716 0.03946301778804395 0.05697341617686202 0.01523951891404263 0.01208109752309472 0.009999999999999787 0.04136314717442291 0.1270658971493295 0.07610367541101892 0.009999999999999787 0.0121690348438408 0.01000598015951049 0.03028268822390956 0.009999999999999787 0.0214905442940565 0.1114750424728417 0.1174095438366676 0.009999999999999787 0.0304952420633664 0.03230075237117613 -0.06983492018343151 -0.160800556572028 0.03533668330933104 -0.001794928747528068 0.04721821842633844 -0.02647078897597083 0.01000002280997514 0.1069992913175895 0.01000000484554309 0.01000000000000001 0.01178580719858924 0.01709763571930867 0.1525722746018422 0.01000000000000001 0.01000000000000001 0.02455119118546556 0.01000000259341527 0.01000000000000001 0.1499762666857705 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1149935543281965 0.01000000000000001 0.07638452834049174 -2.533430780369242 -0.01372149581295462 0.03463920135668518 -2.931846650943954 0.3754309313456834 -4.933492857464121 0.2520657514309607 -1.64600475481375 -1.263895972021406 0.144252350999731 -0.1407771811025913 1.389247208265997 -0.2203080099705163 0.2530119733208784 -0.01409939763091429 0.2906858813516514 -0.2006587292726919 +0.2019780522736529 -0.348822741364792 0.001946004197520779 0.1093308357346556 0.5197541089356394 0.9465063543288825 0.2976234220718443 -0.04718611415208462 -0.09639056674071167 0.2563069581726602 0.9035089414590836 0.5470463872104991 0.5470463872104991 1.317862768772325 1.317862768772325 -0.07976417313908035 -0.5089782059181873 0.1658870372531593 0.004589825989161689 2.417397765581313 2.365032070748922 -0.409539686698619 0.0009342227314097507 -1.573667726679775 0.005131223369970961 -0.7346727658131533 2.762117629948576 0.5654024536631468 2.049658704725834 0.1379750640156461 1.280874640169065 -1.06359119147968 1.537602796219411 -1.729267537807844 -1.729267537807845 -7.944309367850043 -7.944309367850043 -1.105290382145758 -1.667501759385343 -7.083320425305962 -0.2056610534928209 1.631483288790708 2.193823870708755 -2.493739031904459 0.02424178256805387 0.080787413563562 -0.0583949986391068 -0.1225084756673764 -0.02295764755405649 0.01338353747739385 0.005222359930728171 0.05497344294958451 0.01000002421182966 0.04275522824742595 0.1211844444355057 0.1070260411389805 0.01000003819542883 0.02561074910548999 0.01837271223037362 0.03252251371870729 0.03465510428625329 0.04963259241510443 0.01626695652577936 0.02695736768254609 0.1963761700303412 0.1565378738426122 0.009999999999999787 0.02627790481207803 0.02069886924672826 0.02626414685976508 0.03041978435601589 0.04866909849987255 0.0125465404732239 0.01314653462899207 0.009999999999999787 0.04138749579549517 0.1332347517973669 0.08351274413948495 0.009999999999999787 0.0106818898064005 0.01000287947243272 0.03198112476958714 0.009999999999999787 0.02136591285011935 0.1140495206379599 0.1197566983546179 0.009999999999999787 0.02881286247887127 0.005464524601928256 -0.07413211500269801 -0.1632899327848758 0.04514023028378722 0.004693057551792168 0.05668347615339164 -0.03945832001405969 0.01000004089820095 0.09453678164025403 0.01000000245365185 0.03998787369746382 0.01089295976041504 0.02066942606200228 0.128890581086348 0.01000000000000001 0.01000000000000001 0.0184091277298184 0.01000000000000001 0.01000000000000001 0.1338068967506147 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.119299009949673 0.01000000000000001 0.03743926044120123 -2.560102538814548 0.01298327627323092 2.026589516798644 -3.010581012430887 0.3886793133885758 -4.717630170983726 0.2487816910033649 -1.005458573272961 -1.12086040435844 0.1646994565248532 0.07500940713617396 0.7046154443439983 -0.2077389911885912 0.2378904080798586 -0.02125472552137889 0.3859419479037548 -0.2670154730485038 +0.2089428126968823 -0.3596188078589684 0.001911658799006233 0.1041513657211732 0.5390453749926034 0.9504251022313874 0.3118463285836044 -0.04605876430417455 -0.08753166896639186 0.2494706061959069 0.9129616436547754 0.5359614830324766 0.5359614830324766 1.260816325290778 1.260816325290778 -0.08776374747997195 -0.5205567638156026 0.1168743750778689 0.002974781692591133 2.428304768976911 2.380312227024076 -0.4224110728496546 0.001032721848694074 -1.522140594773567 -0.01410590420244207 -0.750358349505273 2.777121587132835 0.5582094959333062 2.02817043890494 0.1845476625702442 1.259940162658397 -0.9035638570161677 1.176895785887781 -1.446359310071683 -1.446359310071684 -8.427587600487788 -8.427587600487788 -1.183118225711314 -1.652547651201484 -6.930717469625612 -0.2593930665765436 1.503701297483619 2.18720768261727 -1.066469274594738 -0.006382150183128044 0.06469076785118055 -0.06163557856026536 -0.1300976101436429 -0.009757076515336749 0.01242262732460997 0.01539028220340066 0.03672489994791528 0.01000003805927863 0.04300951632328465 0.1065147285193184 0.08977010052346657 0.01000001943476558 0.02564387060038786 0.03766703200753696 0.0505755130987664 0.0226519936912819 0.03338056426993186 0.01951414747223712 0.02971279864348242 0.1639154827514986 0.1253984115816631 0.009999999999999787 0.02789034667394708 0.01533825495079411 0.0215809713747368 0.02272654882441039 0.04135912486415183 0.0112629776168105 0.01441708425694754 0.009999999999999787 0.04139448421189851 0.1295756021218217 0.08544885948607073 0.009999999999999787 0.009261420759265171 0.01000142228965295 0.03364067639502455 0.009999999999999787 0.02120053593217719 0.117388108586181 0.1228220136302411 0.009999999999999787 0.02699088525266946 -0.02137170316731973 -0.07842930982196461 -0.1657793089977236 0.05494377725824329 0.01118104385111252 0.06614873388044473 -0.05244585105214855 0.01000005898642653 0.08207427196291872 0.01000000006176083 0.06997574886657543 0.01000011232224085 0.02424121640469601 0.1052088875708539 0.01000000000000001 0.01000000000000001 0.01226706427417135 0.01000000000000001 0.01000000000000001 0.117637526815459 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1236044655711498 0.01000000000000001 0.04299969917336889 -2.356779389416967 -0.01210414246971499 2.985301899933814 -1.369756904006065 0.4026984643546304 -4.168896641447294 0.2079968431829498 -0.334251553837049 -0.9783602233380089 0.2031524450727169 -0.1457623359879233 -0.1823656107272033 -0.2011613383699919 0.2418457829933273 -0.02579986442817881 0.4969128624505543 -0.2407455759679098 +0.2159075731201117 -0.3700754106646675 0.001752963557319465 0.09889933794717365 0.5584300885525995 0.9542516098592118 0.3259168772574257 -0.04463330807969257 -0.07886688095222594 0.2437747636615564 0.9200131706575565 0.5269686713179351 0.5269686713179351 1.20058904230482 1.20058904230482 -0.09615786484584277 -0.5319591330100326 0.06991076397279983 0.001064738656295638 2.438410749863666 2.395408683371649 -0.4233012274769719 0.00112538265456541 -1.482088857080099 -0.03121861986722818 -0.7572283202567922 2.789075498261944 0.5389417484312404 2.013402635687828 0.2241622278361914 1.227164100748437 -0.7322202124359136 0.8515697601962149 -1.142055844504055 -1.142055844504055 -8.861756097251003 -8.861756097251003 -1.222409051139422 -1.620180599570578 -6.56136074415745 -0.2829801648787087 1.402640883556561 2.144732689904861 0.7798839515872933 0.02354925996496071 0.05148582853396544 -0.06510063567161284 -0.1363550492908274 0.001356684792472418 0.01235158693601068 0.02370050097970466 0.022106715684163 0.01000005524765379 0.04317476950211185 0.09575781331942101 0.07460384032750023 0.0100000090168173 0.02570411648207882 0.06031192448132883 0.07174287379856992 0.01621698869294441 0.02604695922232336 0.02200100133614225 0.03207260950956092 0.1388979886073627 0.0989391311756056 0.009999999999999787 0.0293296569046908 0.01259491089064824 0.01957283375483954 0.01709564936537955 0.03603674965233949 0.01061013310905956 0.01582556974175819 0.009999999999999787 0.0413295309850259 0.1180405884702953 0.08151609381040315 0.009999999999999787 0.007871584099280682 0.01000068253435016 0.03527405964447361 0.009999999999999787 0.0210401944728833 0.1210628213217739 0.1264239328208276 0.009999999999999787 0.02512432097585915 0.00367688621698814 -0.08309653439201775 -0.1638124300186671 0.04887409304625234 0.01282736899518211 0.05699159914092478 -0.03656402277827436 0.01000008152548515 0.08283502013519217 0.01000000000000001 0.09655966111912029 0.01000009184386386 0.02454463387153039 0.1032861474223149 0.01000000000000001 0.01000000000000001 0.01113359108651457 0.01000000000000001 0.01000000000000001 0.09047299527166774 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1276616821065907 0.01000000000000001 0.01158837328215024 -1.989659299020969 0.02006058463589094 3.085824011943035 -0.7634977155724765 0.2742647551039707 -3.423716873881606 0.2070974209350328 -0.2288419526563507 -0.5558982478877483 0.2017294943387791 0.05889169089377979 -0.9587937942309308 -0.1972154188397618 0.2280865265709915 -0.01964950659501288 0.5284710185683216 -0.284966047761712 +0.2228723335433411 -0.3802846610435582 0.001480230446654573 0.09361168920046037 0.577891464384559 0.9579090631699949 0.3399070442686893 -0.04294498383799095 -0.07045284241519356 0.2392688113232069 0.9248730474802898 0.5199690563414787 0.5199690563414787 1.1374531674824 1.1374531674824 -0.1047226927269005 -0.543103162868569 0.02539402349005071 -0.0008818235013299258 2.447902914855166 2.410143354072545 -0.4119790408110764 0.001229238146380762 -1.451110893311476 -0.04685300878258269 -0.7605872649456944 2.799135918511061 0.5096638728911889 2.005088047645913 0.2600328196317241 1.187934964520102 -0.5618750913723507 0.5475417099788569 -0.8739931073642015 -0.8739931073642015 -9.262818580363225 -9.262818580363225 -1.232139442469661 -1.578362949877469 -6.228074313605552 -0.2698798942759471 1.327437978159472 2.083317347650337 2.440425593621112 -0.003167485136267523 0.04520423078966918 -0.06878326789896594 -0.1411240044856039 0.009360488227998065 0.012591365146287 0.02884937859711734 0.01300859746274163 0.01000007527170732 0.04327030978840307 0.09015930310588161 0.06217633230675323 0.0100000033946146 0.02579315083246492 0.08402096679027649 0.09345799515768327 0.01309130089909516 0.02236461242259669 0.02334677266416207 0.03352370507804414 0.1232231802476291 0.07778332939815691 0.009999999999999787 0.03079993779846513 0.01128700729307885 0.01858224036729084 0.01376909822040417 0.03353374412345778 0.01030222731720087 0.0172329489303511 0.009999999999999787 0.04126456369344123 0.09923286472220072 0.0719285803193781 0.009999999999999787 0.006524453766926008 0.01000033538914291 0.03683021132637698 0.009999999999999787 0.02093510559659739 0.1248884674328687 0.1300587910838145 0.009999999999999787 0.02316479083514267 0.02872547560129624 -0.08776375896207089 -0.1618455510396105 0.04280440883426129 0.0144736941392517 0.0478344644014046 -0.02068219450440018 0.01000010406454377 0.08359576830746573 0.01000000000000001 0.1231435733716653 0.01000007136548708 0.02484805133836487 0.1013634072737758 0.01000000000000001 0.01000000000000001 0.01000011789885769 0.01000000000000001 0.01000000000000001 0.06330846372787635 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1317188986420316 0.01000000000000001 0.02298095099044591 -1.570319551948692 -0.003837248296292564 3.142596803411728 -0.3212619016942653 0.1417518580773638 -2.645950177704406 0.2168903760977491 -0.04205364018843265 -0.1687502690023301 0.2028347611601989 -0.1457663748270393 -1.806182503886198 -0.1888977930241256 0.219667972742935 -0.009933537465950418 0.5063716436005449 -0.2674416453915238 +0.2298370939665705 -0.3902788673096822 0.001101976056701037 0.0883024824844143 0.5974194664298071 0.9613349224750696 0.3538311544615809 -0.04101040556534352 -0.06232528335462639 0.2359058968700873 0.9276286040848705 0.5146391872144558 0.5146391872144558 1.071594956584406 1.071594956584406 -0.1132683285711069 -0.5539354905902629 -0.0174363964169606 -0.00263404812973933 2.456953201950396 2.424425704597439 -0.3907426267759204 0.001330181025791966 -1.420468225138769 -0.06171639543798335 -0.7634006093538499 2.808291714777512 0.4727595530137445 1.994733923629944 0.294959464298203 1.145019655881637 -0.4043251903656802 0.2485343218068619 -0.6617792531081521 -0.6617792531081523 -9.642844197796176 -9.642844197796171 -1.221002472818516 -1.532460860837481 -6.079172621376377 -0.2475795500713924 1.274275013968735 2.018082234119388 3.615376435919664 0.02396971481046517 0.04137232966602111 -0.0718590083059647 -0.1460964596840317 0.01184897443566957 0.01215492716657796 0.03190289792444112 0.006382044290212718 0.01000010517455729 0.04334642911996234 0.08103072348824991 0.05205556069647965 0.009999999999999787 0.02588473643976474 0.1038487080179702 0.1125076887380643 0.01149759869777833 0.02070284893726715 0.02516419965862982 0.03486936759442916 0.1044195800112924 0.06105656842002105 0.009999999999999787 0.03244576031139168 0.01062176464453035 0.01820175668468327 0.01182786336802 0.03289266103225286 0.01014578451505699 0.01860551631574747 0.009999999999999787 0.04111537253263986 0.08469376105991078 0.06239109884016036 0.009999999999999787 0.005243241469066184 0.01000015975932378 0.03829191170393198 0.009999999999999787 0.02088589812789277 0.1268074336258884 0.1330045251956982 0.009999999999999787 0.02118422142195087 0.01950085845361305 -0.08399791591447364 -0.1751011996833288 0.004642029958721983 0.006124809223513661 0.04382068423419228 -0.02655084069641839 0.01000015726884507 0.05882984282262815 0.01000000000000001 0.1315660754972493 0.01000003526134186 0.02871120147579342 0.06263611598056718 0.01000000000000001 0.01000000000000001 0.01000008744244629 0.01000000000000001 0.01000000000000001 0.07037209653225274 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.127223556898967 0.01000000000000001 0.00334649861301348 -1.372983103056556 0.02246161271764873 2.333198163910805 -0.167055029802527 0.2502736730583069 -2.219148280382712 0.2504263583920193 -0.05353085118848844 -0.002728856711647645 0.1902040056350884 0.04096518592032221 -0.9161925690476435 -0.1784619241918599 0.2014317566199557 -0.008485513547200446 0.3375047767878029 -0.2928479532349225 +0.2368018543897999 -0.4000939989252359 0.0006212484102348093 0.08298629049776851 0.6170063943004296 0.9644756275899473 0.3677118948001636 -0.03884387633323527 -0.05451656347252154 0.2336297060009604 0.9284017427584641 0.5106776918368734 0.5106776918368734 1.00321898801726 1.00321898801726 -0.1217192201971855 -0.5644525826102993 -0.05939831138430796 -0.0045295516089916 2.46569242220571 2.438255401496832 -0.3622098149086308 0.001449118362778279 -1.39968783214295 -0.07627922024651301 -0.762593154644569 2.816057053423845 0.4277790203290994 1.992633964018272 0.3266398370624728 1.096377133642959 -0.2498116411321734 -0.02172925142876814 -0.4810510199949634 -0.4810510199949634 -9.985817654598527 -9.985817654598527 -1.204932700995859 -1.487839893087517 -5.978674510349244 -0.3110238366639337 1.238112051344136 1.953333371749905 4.535658563232817 0.00199980174120773 0.03656442407156391 -0.07369651995955806 -0.1525680544254122 0.006980235473001972 0.01028644945936907 0.03367733712328924 -0.0001073385589149822 0.01000014742641264 0.04337917634248845 0.06429555635673267 0.04253562630233265 0.009999999999999787 0.0259991163784159 0.1180038401046062 0.1260347534522928 0.0107422542510931 0.01988264896719061 0.02802675007442534 0.03708797516835949 0.0757805181191511 0.04601021068536282 0.009999999999999787 0.03421446512147197 0.01030799098815693 0.01802706982506486 0.01090615947592521 0.03367144276234058 0.01007219133343007 0.01986697174599161 0.009999999999999787 0.04097212340221823 0.07999194307564395 0.05939500241517059 0.009999999999999787 0.004046269025840488 0.01000007810000758 0.03965497512780347 0.009999999999999787 0.02075716777837577 0.125895365974555 0.1347319286506181 0.009999999999999787 0.01920327117437948 0.01027624130592997 -0.08023207286687639 -0.188356848327047 -0.03352034891681743 -0.00222407569222427 0.03980690406697984 -0.03241948688843654 0.01000021047314636 0.03406391733779046 0.01000000000000001 0.1399885776228331 0.01000000000000001 0.03257435161322209 0.02390882468735855 0.01000000000000001 0.01000000000000001 0.01000005698603468 0.01000000000000001 0.01000000142099411 0.07743572933662912 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1227282151559024 0.01000000000000001 0.0105262199079768 -1.397783188972745 0.002708369920534595 1.556719176858838 -0.07959859552388639 0.3924266851830332 -2.163405727836202 0.2521772459079766 0.0170429576803429 0.2389779900285011 0.1709278617425707 -0.1440600619120939 0.07224106194703353 -0.1647074836638392 0.1913433936655524 -0.0327693210359644 0.1565159177015786 -0.2675510057566988 +0.2437666148130294 -0.4098280903006506 4.250698039243161e-05 0.07770024157945965 0.6366401303961484 0.9672710651324601 0.3816363611412155 -0.03647936737405111 -0.0470752362059117 0.2324511322070166 0.9274728766769238 0.5079966624829195 0.5079966624829195 0.932665606236295 0.932665606236295 -0.130078718906685 -0.5746774155930043 -0.1003879590026928 -0.007297626264334944 2.474233009318451 2.451649059528166 -0.3275336828594702 0.001587313353605069 -1.381908189452964 -0.0882413540766338 -0.7542641644257666 2.822243216294012 0.3748172260978744 1.989186031453005 0.3508839321842054 1.039369932504831 -0.1064247497850093 -0.2570191767374008 -0.2829901876601499 -0.2829901876601499 -10.26699007939966 -10.26699007939967 -1.196587076916736 -1.446467666906805 -5.775011329812563 -0.5028825763269253 1.21763234586101 1.890612511927209 5.379573959523696 0.02865675539194967 0.0294842700785618 -0.07347602363290306 -0.1630174638631829 -0.005730762977531612 0.009304291811881971 0.0332864304666054 -0.007846579996980019 0.01400626982949493 0.04662470069681612 0.04668844638792535 0.03277983685308072 0.009999999999999787 0.02611807669643973 0.1338147410420332 0.1385807672531185 0.0103582903473991 0.01950497383628003 0.0350351763120238 0.04151177958420948 0.04946652000278373 0.03142113876735353 0.009999999999999787 0.03594761895487464 0.01014856549765408 0.01807330951144426 0.01043754734241542 0.03520591027311148 0.01003480812391189 0.0209827066703161 0.01000000013615931 0.04077558310652929 0.07033660371338302 0.05503365555845541 0.009999999999999787 0.00300753089506145 0.01000949565370668 0.04094447790217703 0.009999999999999787 0.02039456489028479 0.1213974492965022 0.1351220879111099 0.009999999999999787 0.01738963607033917 -0.01484317985672706 -0.06517197689609472 -0.231272162062701 -0.09161435519864358 0.01149994496573126 0.02370201915137693 -0.05293309849160599 0.02427871581364671 0.02203195509736622 0.01000000000000001 0.1669147750856349 0.01000000000000001 0.05010955451025001 0.01695440800994352 0.01000000000000001 0.01000000000000001 0.01000004927136333 0.01000000000000001 0.01000000109391752 0.04371790720569313 0.01000000000000001 0.010033565075384 0.01000000000000001 0.1093698182063129 0.01000000000000001 0.8544462904309257 -1.386473581259346 0.02111100902924141 2.080550576615505 -0.0274187763214207 0.8636340211658193 -1.965450162375145 0.2444978732996217 0.004806655904940314 0.2262944626449226 0.1505285696592734 0.0275152330066599 -1.320601180319288 -0.133155867339239 0.1775766803849353 -0.07209464426885025 -0.06966971262240464 -0.2505111668060704 +0.2507313752362588 -0.4191532521255708 -0.0005846384213099576 0.07249485212186624 0.6563227667783349 0.9696946829303248 0.3951872823622065 -0.03397668565425294 -0.04005401411625931 0.2318993531726337 0.9246542210630535 0.5068171636052732 0.5068171636052732 0.8603063552690964 0.8603063552690964 -0.1384011298013048 -0.5845753505262139 -0.1396060735513998 -0.01179949266540303 2.482698451674215 2.464560420088241 -0.2878650173686546 0.001722544422814831 -1.282259657850004 -0.0901789006149385 -0.7394309908894714 2.830099255450847 0.3210072946528806 1.885366779707327 0.3663175150341536 0.9757397674083768 -0.06982131224277532 -0.5644010498575522 -0.04987165844837671 -0.04987165844837671 -10.50439089372027 -10.50439089372027 -1.194282567291783 -1.393967877509162 -5.469961227466293 -0.8088993242069333 1.216533489436712 1.814836871669467 5.969314033979957 0.001148682265709411 0.01913926673511668 -0.07056977201433456 -0.1793427376344958 -0.02665543675437521 0.01098374210196074 0.03005151849420562 -0.01789779972938765 0.02332987428274791 0.05434775895441391 0.03137018396672264 0.02346250872271316 0.009999999999999787 0.02614913388227214 0.1540798091540925 0.1554971957303737 0.01017745103990775 0.01952071836264935 0.04722311151367853 0.04891908939922907 0.03176263037115445 0.01947573515333723 0.009999999999999787 0.03760603443785282 0.01007356744771126 0.0182134201411408 0.01021674022948904 0.03716676848233913 0.01001723351232009 0.02197855696021778 0.01000000052158878 0.04051814497279072 0.04876590269523184 0.04105598987682679 0.009999999999999787 0.002197330361866534 0.01003171054521257 0.04210940196345314 0.009999999999999787 0.01974263068961912 0.112912579806252 0.1334105519178488 0.009999999999999787 0.01575195323779166 -0.03996260101938404 -0.05011188092531293 -0.274187475798355 -0.1497083614804698 0.02522396562368689 0.007597134235774128 -0.07344671009477538 0.03855722115414706 0.01000000000000001 0.01000000000000001 0.1938409725484365 0.01000000000000001 0.06764475740727793 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.0100000415566921 0.01000000000000001 0.01000000076684082 0.01000008507475725 0.01000000000000001 0.01006713146411109 0.01000000000000001 0.09601142125672346 0.01000000000000001 1.296294467830168 -1.271874674587665 -0.02253388590811325 2.811727076845775 0.03337575919511446 1.249169837414945 -1.404250166595921 0.2307153876713102 0.04399892963629726 0.3614203060537047 0.136501800505197 -0.1615474798267131 -2.689180219284117 -0.09908064693306813 0.1555693812148881 -0.1158536756976801 -0.4470067094486582 -0.2170245667095806 +0.2576961356594882 -0.4272034747959212 -0.001170336285611473 0.06741049554817424 0.6760779497358409 0.9717651466805046 0.4073224822159021 -0.0313982597513851 -0.03349241856893359 0.2309040036867271 0.9190681546638135 0.5074507577015512 0.5074507577015512 0.7863814750933211 0.7863814750933211 -0.1467018224332426 -0.5940115848901717 -0.1762844975690379 -0.0187965527001337 2.491223583211556 2.476849587097234 -0.2452136333919057 0.001792714595373113 -1.012048081521986 -0.07647045057847812 -0.7213211595480193 2.842929203647363 0.2738179038467106 1.577782484539622 0.3734292747195447 0.9094387927547212 -0.229649129017369 -1.051210090888201 0.2610566260153515 0.2610566260153515 -10.69640169593585 -10.69640169593584 -1.189457670509725 -1.311259304674046 -4.966588991657562 -1.199048138954963 1.229015191252479 1.707588609906857 6.203420642218219 0.01279740428273124 0.007617321680562394 -0.06801571252633343 -0.1956818039807735 -0.04826159689823051 0.009574378689709562 0.0275587925614289 -0.0292795917806159 0.03121825964021419 0.06108717978005673 0.02612446378716982 0.01645752224441832 0.009999999999999787 0.02597053609134203 0.1807750100413368 0.1795090826611712 0.01008557967538692 0.02003310953447368 0.05121368537575099 0.05434958718222394 0.02083284901096771 0.01215677966557704 0.02722308445524124 0.05481234891846665 0.01003547019355011 0.01851415528837341 0.01010455747015593 0.03943741152171398 0.01000830656975271 0.02287206669386066 0.01000000011894775 0.04003421100448668 0.02973911217599934 0.02673285417515459 0.009999999999999787 0.001557748024064054 0.02071445357587187 0.05336122965459866 0.009999999999999787 0.01878852891938987 0.103681835412901 0.129549148090438 0.009999999999999787 0.01417846235438791 -0.04915354945315353 -0.06235736349444232 -0.2658002862923068 -0.1432111964734226 -0.017582087021975 0.02437840677629199 -0.08802797626270525 0.03938268528228162 0.02991810986267507 0.01000000000000001 0.240435852602415 0.01000000000000001 0.0449633927595966 0.01000000000000001 0.07231685325240766 0.01000000000000001 0.01000003706461894 0.01000000000000001 0.01000000000000001 0.01000005179550978 0.01000000290085989 0.04836634916446259 0.01000000000000001 0.08876076826283041 0.01000000000000001 0.664329415307781 -0.6903658760726594 -0.03115393308904119 4.075468772760804 0.1116119030188272 0.1943917224402497 -0.7140163691016757 4.227183723302667 0.05010384387266916 0.3123821270936135 0.1191606210549702 -0.02755710637048983 -1.485019710044404 -0.08535885374104339 2.800608787702829 -0.1541310159365069 -0.6399198269724463 -0.2287872365342601 +0.2646608960827176 -0.4330083137768517 -0.001628389311155054 0.06243704275670181 0.6959252447390933 0.9735126101318987 0.4168642468274166 -0.0287843800698413 -0.02737271563018773 0.2285103690319041 0.9098509447195831 0.510860886342861 0.510860886342861 0.711693243226494 0.711693243226494 -0.1549712940258345 -0.6027781453184682 -0.2074507262190766 -0.02848316462141076 2.499782839580754 2.488255342366771 -0.202499514690075 0.00181438774569731 -0.6374754720213371 -0.0535244197692224 -0.7075838703894646 2.8565501425314 0.2282545568258882 1.140636140125244 0.3764984190469951 0.8488459241951181 -0.4713527602843715 -1.607119915807797 0.7474377610724146 0.7474377610724146 -10.72359489922049 -10.72359489922049 -1.18531713633021 -1.201664368131111 -3.887079683018827 -1.581230811063603 1.226332773198553 1.561156433070325 5.987328932180422 -0.01277771843774334 -0.003482746324319663 -0.06814015847984534 -0.2075457716655347 -0.06478145713781602 0.0006728066033874214 0.02855438468603877 -0.04124044288637529 0.0354143025162923 0.06395441734281127 0.03368050647849996 0.01453291811550983 0.009999999999999787 0.02568172009171965 0.2147028823416401 0.2121543133976882 0.01004237632143745 0.02104546266839247 0.0419132445004311 0.0500130026548411 0.01540569574548512 0.009299213338672008 0.06622769080898649 0.08975981890569429 0.01001755959088602 0.01901920863520523 0.01005179447272297 0.04182127471511476 0.01000411036494731 0.02362562341154506 0.009999999999999787 0.039435887282671 0.01992661510776728 0.01951831474570653 0.0100000023358362 0.000997497910852907 0.04518052443131237 0.07729137512293249 0.009999999999999787 0.01765130918064184 0.09523610158760043 0.1248020248110397 0.009999999999999787 0.01264907538379667 -0.05834449788692303 -0.07460284606357181 -0.2574130967862588 -0.1367140314663755 -0.06038813966763684 0.04115967931680975 -0.1026092424306351 0.04020814941041606 0.04983622686840816 0.01000000000000001 0.2870307326563933 0.01000000000000001 0.02228202811191515 0.01000000182334371 0.1346337115045876 0.01000000000000001 0.01000003257254589 0.01000000000000001 0.01000000000000001 0.01000001851626253 0.01000000622929131 0.08666556686481397 0.01000000000000001 0.08151011526893737 0.01000000000000001 0.1843586574255429 0.1870074809394981 -0.05418413717391089 5.290877336271641 0.1769447929399301 -1.555544673231749 -0.1231180415072881 5.325264066704028 0.1026709261408393 0.3939325956757086 0.09631290456092871 -0.1943956315541777 -0.6478763292550245 -0.07629961218844472 3.796274016009213 -0.1684378156858705 -0.7013498304015707 -0.2043596909254261 +0.271625656505947 -0.4362276538515433 -0.001921522635163431 0.05751777952934356 0.7158509904560257 0.9749208279757955 0.4233527325519733 -0.02614758940732864 -0.02162696643222706 0.2246743321554994 0.8970591735609146 0.5183420392090317 0.5183420392090317 0.6377102997556774 0.6377102997556774 -0.1632670707863362 -0.6107426047856208 -0.2278734348651903 -0.04054513408645244 2.508206384491318 2.498494871834314 -0.1639895390638877 0.001809487636386553 -0.2915016180245789 -0.03115016084409294 -0.7070131328946048 2.864405714706689 0.1755419904298123 0.7264615564994488 0.3813717192195556 0.8033321678695451 -0.6154542908640219 -2.044056809766236 1.407459455147031 1.407459455147031 -10.46344116499947 -10.46344116499946 -1.197408345112167 -1.081361728831964 -1.978702161967522 -1.850589248525555 1.185799901241577 1.374616141418616 5.05470239097159 0.005661532150962767 -0.009034251917997338 -0.07050949913755389 -0.2148675580562442 -0.07417669587815601 -0.00974632551914878 0.03383648624444824 -0.04991023672127604 0.03373367795294469 0.06166210056392396 0.03636775336064835 0.01712192844547111 0.009999999999999787 0.02545515794581688 0.251271923802074 0.2495108394942971 0.01002043065265301 0.0223336624705901 0.03055988500215889 0.03772850191083243 0.01262796374702502 0.009777651769109053 0.0967478352293929 0.1189119503538865 0.01000846189987792 0.01977573685250089 0.01002499044156746 0.04423508008890131 0.01000197905528699 0.02420349932632471 0.009999999999999787 0.03879352289182947 0.01485836817516351 0.01591789135043786 0.01000000766776798 0.0005319781032699922 0.08363076991453511 0.1087071795285119 0.009999999999999787 0.01652952343650549 0.08790312389510468 0.119729231875763 0.009999999999999787 0.01135374365781372 -0.01154750070261851 -0.0875412901625085 -0.2393541486992139 -0.09811945783735598 -0.05391346650029311 0.07350317788736693 -0.07672047503388418 0.0251040698546392 0.02991812630539403 0.01000000000000001 0.3291548447542494 0.01000000000000001 0.01614101210313579 0.01000000000000001 0.1295963130098813 0.01000000000000001 0.01000002780355791 0.01000000000000001 0.01000000000000001 0.01000001183457355 0.01000001799581918 0.1605874854135233 0.01000000000000001 0.07702236964255282 0.01000000000000001 -0.8023583270415747 0.4732878242647608 -0.0187463813622307 5.410187824694868 0.1906412211663651 -1.830452565338654 0.2276287521941638 2.968089905458259 0.1147273911474658 0.3179274133146993 0.07024821515976863 -0.03911475114510438 -0.3817199067320264 -0.05374614704740627 5.096567584250466 -0.1490882676634648 -0.7527653170191737 -0.1620108202228913 +0.2785904169291764 -0.4371316370852907 -0.002069239165925385 0.05256108304611606 0.7358122348621339 0.9759496469941431 0.4270373416126598 -0.02346244951679832 -0.0161516310675287 0.2201428316888601 0.8816861550985653 0.5305581963943924 0.5305581963943924 0.5667490347444586 0.5667490347444586 -0.1716577357541142 -0.6177846178049378 -0.2350297951416476 -0.05381671733879756 2.516206136755531 2.507338873970967 -0.1323196293631121 0.001813725547782141 0.02740219792335408 -0.01176654601339955 -0.7183332009023482 2.866743943041982 0.1193064229364484 0.3354751601344015 0.3903823791766592 0.7711939112525816 -0.6710620246412429 -2.348350668014528 2.107141191327313 2.107141191327313 -9.855889974461643 -9.855889974461643 -1.212565225714312 -0.9367747260537058 -0.07751623401699392 -1.928583401434008 1.104636513577098 1.160416925586315 4.023136193371543 -0.01015364520276307 -0.005111427225186205 -0.07479169635338323 -0.2175957945150242 -0.07488646495469187 -0.01710792050814813 0.04401631361587288 -0.05232542524760397 0.02516836585414861 0.05333877813309762 0.02751322866937622 0.01996714662670085 0.009999999999999787 0.02531095338711076 0.2887038451997141 0.2871502433444921 0.0100101140866462 0.0236685013703104 0.02166780041762406 0.02648835934728311 0.01130342999462375 0.01201200318873852 0.1097882202661626 0.1300178103642931 0.01000418545471549 0.02061944774531188 0.01001238734845344 0.04651049858640111 0.01000097751543683 0.02461276031914661 0.009999999999999787 0.03820780736082874 0.01241335695808754 0.01426105069678441 0.0100000164098164 0.00029944395121273 0.1359082673140648 0.1464939243701071 0.009999999999999787 0.01563873782738101 0.08182124810569036 0.1143524091138062 0.009999999999999787 0.01047030351758726 0.03524949648168596 -0.1004797342614453 -0.221295200612169 -0.05952488420833646 -0.04743879333294937 0.1058466764579239 -0.05083170763713324 0.01000000000000001 0.01000002574238013 0.01000000000000001 0.3712789568521053 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1245589145151746 0.01000000000000001 0.01000002303457015 0.01000000000000001 0.01000000000000001 0.01000000515288457 0.01000002976234715 0.2345094039622329 0.01000000000000001 0.07253462401616806 0.01000000000000001 -1.547504403338785 0.2605801052846337 -0.03053433956644051 5.372108487499082 0.1903376721415321 -1.255646868546762 0.3811097804033903 0.1430983233316922 0.1277062956901695 0.3541934322659372 0.04789345818081277 -0.178128349880361 -0.08975727746018543 -0.009395471525329296 5.625752720074911 -0.1021038369117544 -0.7886550093006526 -0.08608093882918615 +0.2855551773524058 -0.4357339216910732 -0.002092385510851891 0.04750050143080342 0.7557779288960331 0.9765957437094857 0.4278819333706627 -0.02070068671886682 -0.01087172163841199 0.2153102263378726 0.864482753336913 0.5475171547099036 0.5475171547099036 0.5013259476990726 0.5013259476990726 -0.1800936437801495 -0.623663545324147 -0.2323454707642361 -0.06682236305171418 2.52349221064396 2.514627843625653 -0.1070124968823407 0.001819574359753773 0.3685858904905226 0.004571634195583485 -0.735775510167217 2.865402329600693 0.06610603287786132 -0.08813283973087582 0.4027030434873149 0.7464415045760013 -0.6988019327012069 -2.569275751826601 2.771976945785921 2.771976945785922 -8.877237992212038 -8.87723799221204 -1.20811783646861 -0.742503574783232 0.7698670239615319 -1.786813236338746 0.9757865182720868 0.9300040127407803 3.263879736755104 0.00561194511541574 0.006866054191480675 -0.07960454778871995 -0.2176424779764936 -0.07269908982331685 -0.02676342404683885 0.05921593831537786 -0.05269473447261541 0.01851591895574112 0.04585002380382708 0.01915781294574881 0.02006562061906259 0.009999999999999787 0.02518524464395799 0.3317446360690517 0.32746766440897 0.01000487358708435 0.02502225928648416 0.01572512139784044 0.01955967309682771 0.0106297145430907 0.01389227988453001 0.119631798521747 0.1296423781014115 0.0100020132299723 0.02149098970517649 0.01000598464442604 0.048570998723243 0.01000046966422596 0.02489206862465609 0.009999999999999787 0.03760540981333982 0.01116793813841044 0.01359496606602484 0.01000003126378779 0.000368954276865896 0.1958240731847569 0.1844577876314606 0.009999999999999787 0.01513756493491769 0.07680931842884364 0.108667147895749 0.009999999999999787 0.01005530347181427 0.08572471235879398 -0.1023765015179433 -0.2146151748954448 -0.06582036543114039 -0.0924108279884252 0.1492986651064881 -0.05770407031893443 0.01371977701910621 0.01176965359192872 0.01000000000000001 0.4403472609047181 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1386711833614276 0.01000000000000001 0.01000002300200875 0.01000000000000001 0.01000000000000001 0.01000000453714855 0.0100000579370263 0.3204327672385913 0.01000000000000001 0.06965902756186648 0.01000000000000001 -0.6796065636075301 -0.17770906448754 -0.007651179875053349 6.179020738054342 0.1986207310286267 -0.7490801740020183 0.1767530656609697 -0.1386466527467675 0.1270861368508114 0.2534883428619607 0.03084664386026473 -0.0371468291513557 -0.0830484697848142 0.02787343571761171 5.22665463257968 -0.04461704651055982 -0.8300301216072633 -0.03078219037427798 +0.2925199377756352 -0.4320723494849394 -0.002013194786675054 0.04229937159619679 0.7757091912098684 0.9768687921642667 0.4258766863612413 -0.01785274034119189 -0.005733762123157238 0.2106577284545841 0.8462108022703045 0.5692984515881734 0.5692984515881734 0.4438422147241812 0.4438422147241812 -0.1884615614154699 -0.6280031688500911 -0.2253990991317556 -0.07843728163124641 2.529633375841049 2.520256067672938 -0.08657923814487578 0.001805238300327972 0.6774911239919867 0.01762055788778705 -0.7586930153817537 2.856856533291086 0.01218191673898605 -0.4828842935319697 0.4151308484714136 0.7304315862796908 -0.6193423798289381 -2.655195193903709 3.491923515387646 3.491923515387646 -7.576025747140534 -7.576025747140534 -1.193041111010065 -0.494748324653604 1.146372886624956 -1.529222318304223 0.7758646329224743 0.6835173607833651 2.623555031318142 -0.01594993112540299 0.0258110749310001 -0.08388962969504732 -0.2164717232856233 -0.07204699036603657 -0.0428107146019765 0.07952883680946243 -0.05424100336325033 0.01720576986016109 0.04280461300948168 0.01597489218911363 0.01825220242903303 0.009999999999999787 0.02517530510113319 0.382598238142815 0.372853052615949 0.0100024099679743 0.02643816550439126 0.01284566793078668 0.01584388932052638 0.01031193031101996 0.01472374406231891 0.131224672710907 0.1296502676129654 0.01000099214364125 0.02245266539906776 0.01000297503932401 0.05026418514988817 0.01000023240264669 0.02502201795855008 0.009999999999999787 0.03710127992565582 0.01057874190478181 0.01336146257028448 0.01000005317727881 0.0006670554399357265 0.2621567009025365 0.2186121004364971 0.009999999999999787 0.01497818371357207 0.07277346981549293 0.1029840238315511 0.009999999999999787 0.01007367514385216 0.136199928235902 -0.1042732687744414 -0.2079351491787206 -0.07211584665394449 -0.137382862643901 0.1927506537550522 -0.06457643300073573 0.01743956373934985 0.01353928144147731 0.01000000000000001 0.5094155649573308 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1527834522076803 0.01000000000000001 0.01000002296944713 0.01000000078747609 0.01000000000000001 0.01000000392141243 0.01000008611170555 0.4063561305149496 0.01000000000000001 0.06678343110756491 0.01000000000000001 -0.2715542584484554 -0.2884386882375862 0.002709933973678366 6.827425493721122 0.208184173087 -0.3330300722526991 0.07993286856083195 0.2531729708757471 0.1535890909486822 0.2487156590551033 0.005003548815037544 -0.1499098172126416 0.0344626838179164 0.05624665355272551 4.531802541134339 -0.003954970246250666 -0.788041304299323 0.0383660461298482 +0.2994846981988646 -0.4265872663071684 -0.001855722954648531 0.03691452421393393 0.7955440301548462 0.9767474292886664 0.421467199897736 -0.0149223303185515 -0.0006670643792707764 0.2072363079488055 0.8280198172084363 0.5965741740198895 0.5965741740198895 0.3964074960946289 0.3964074960946289 -0.1967346145586242 -0.6304460591889507 -0.2156632683012254 -0.08801995987085487 2.53408059709504 2.524106527569375 -0.06995391503708026 0.001751724812693478 0.8864330932615507 0.02748192300497498 -0.7884966590055331 2.837884506765173 -0.04753638473966859 -0.7712457779681658 0.4258207377985053 0.7259031130973104 -0.3437267470166572 -2.549148490714568 4.345884749453585 4.345884749453588 -6.020759768095783 -6.020759768095783 -1.18540476197878 -0.2059893328867375 1.627870132125443 -1.200561191576138 0.4922070572984638 0.4185806373993546 2.155867041024201 -0.003923586210166086 0.04291200953864394 -0.08893531492192164 -0.2155501606464432 -0.07481381178907887 -0.05695923155773119 0.09154425220141604 -0.05679010634505977 0.01629068870254935 0.04007115773563008 0.01423122273470145 0.01646976127462807 0.009999999999999787 0.02538591277102231 0.4265614942319402 0.4136572152748448 0.01000115839009386 0.02792006773107136 0.01137805829547034 0.01381638932604456 0.0101504665457437 0.01547297184756724 0.137552925946677 0.1301337034830148 0.01000047345951982 0.02359306350638324 0.010001446793519 0.05152371419758994 0.01153162638043392 0.02552636177137035 0.009999999999999787 0.03671727434586058 0.01027929354513057 0.01326588660014716 0.01000007460299912 0.001108207411730788 0.323325508290436 0.2457512235638539 0.009999999999999787 0.01506931796153044 0.07633876352269242 0.1009173284865468 0.009999999999999787 0.01053429943521822 0.1058296916058873 -0.1189726283275141 -0.2145957689899913 -0.1017429416181894 -0.1059626725232938 0.1021789982088119 -0.07192976722114985 0.01371982787583881 0.01176963813812026 0.01000000000000001 0.5144762605853482 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.139986877397886 0.01000000000000001 0.01000002461920857 0.01544386911080342 0.01000000000000001 0.0100000008568959 0.01000010162230336 0.4482581188587192 0.01000000868494355 0.09203521933361525 0.01000000000000001 -0.494554475819831 -0.2372888465830169 0.04990231264956467 4.783077963848963 0.2164824056547975 -0.2496213354557061 0.1249823925945412 -0.1386092483891598 0.1724980295203072 0.1272105206733348 0.1481286376358057 -0.002403571020922218 -0.03772373847354834 0.07083775012628023 3.23181165762637 0.03319223354670006 0.18627218025079 0.09547448156675671 +0.3064494586220941 -0.4198803789586787 -0.001632016147680826 0.0313037696532974 0.8152252208593409 0.9761996138054241 0.4153021170989146 -0.01192884893576807 0.004396974002176979 0.2061403701125837 0.8109723116297389 0.6299085676913672 0.6299085676913672 0.3603180832633317 0.3603180832633317 -0.205012080556136 -0.630861880009594 -0.2030230955506749 -0.09485438712488348 2.536364330333845 2.526036552841959 -0.05647527629474869 0.001687810000886358 1.028344303850605 0.03664058755600941 -0.8235645011609594 2.812734917859766 -0.1102781771420287 -0.9870211851508606 0.4332424147883867 0.7296679239929693 0.04844302218305785 -2.326833724536453 5.231731023944681 5.231731023944681 -4.318108716578118 -4.318108716578118 -1.194308305889385 0.08734478265315992 1.980383460850746 -0.7400348032427515 0.1545976018345523 0.1320455205023192 1.719953421431343 -0.01893677447248265 0.05142134111044738 -0.09572817712996207 -0.2160008645863924 -0.08244195855401326 -0.06286046428047953 0.08499287500693997 -0.06020870646866117 0.01390085551421372 0.03617799963866819 0.01246822435853234 0.01475351445516537 0.009999999999999787 0.025760853112065 0.456419053652191 0.4379911361280175 0.01000056997295884 0.02944145474145721 0.01068294694368177 0.01236072877285377 0.01007450916286912 0.01632214916645269 0.1355155671671753 0.1273815814781862 0.01000022963983005 0.0248361252102951 0.01000072933496199 0.05223452211592683 0.01511761702440761 0.02720106163696068 0.009999999999999787 0.03648216913554991 0.0101383207519854 0.01317286792061045 0.01000009289390658 0.001659408320147548 0.374844521015774 0.2632166216030947 0.01000000761864417 0.01548325969963793 0.09002192006929022 0.1054631318706765 0.009999999999999787 0.01142542618035813 0.07545945497587236 -0.1336719878805868 -0.221256388801262 -0.1313700365824343 -0.07454248240268668 0.01160734266257157 -0.07928310144156397 0.01000009201232788 0.01000000000000001 0.01000000000000001 0.5195369562133655 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1271903025880917 0.01000000000000001 0.01000002626897001 0.02088773743413064 0.01000000000000001 0.01000000000000001 0.01000011713290128 0.490160107202489 0.0100000217207572 0.1172870075596658 0.01000000000000001 -0.6045744211177879 -0.2694303288177547 0.04989968959241943 2.097832282794151 0.2195226201575405 -0.1688207078058503 0.1086343679824632 -0.6759495483644176 0.1830699604582158 0.09114494706360909 0.3410828915653125 -0.1071516000159011 0.0351970351661712 0.08784829666383992 1.753896482357001 0.08874240068764747 1.110803258130511 0.1619890934782718 +0.3134142190453235 -0.412197274367303 -0.001337419980843357 0.02543902767168316 0.8347265992394788 0.9752216618188281 0.4076433022228185 -0.008901451922794568 0.009503446349308664 0.2077908136640079 0.7955387410947106 0.6691827949851343 0.6691827949851343 0.3363553237210248 0.3363553237210248 -0.2134005765314821 -0.6292819315026756 -0.1889984227300636 -0.09801322562507586 2.536185929799643 2.525885774244779 -0.04558368758449971 0.001595794894166946 1.190407121525378 0.04823617536104496 -0.860309377240795 2.78772504559455 -0.1691018163822195 -1.226731616920064 0.4350663491582751 0.7378598691170346 0.4001287563800244 -2.127956726787023 6.014720875280206 6.014720875280201 -2.596246313026248 -2.596246313026245 -1.211433807713961 0.3804087095965691 2.031251669892367 -0.2235623189601696 -0.21528583164568 -0.1810783407938694 1.411442683314951 -0.0135750482819903 0.03198055076250927 -0.09995010374976587 -0.2137655472463109 -0.0855144921681612 -0.06927837648353119 0.09283358480659398 -0.06388424715308627 0.01189212129278161 0.03251019748964135 0.01119458557228725 0.01274705706310053 0.009999999999999787 0.02613972988775037 0.4591547216557572 0.4404942099672522 0.01000027090174438 0.0309895662839792 0.01032963099647954 0.01120231990392684 0.0100359245503312 0.01690602042469846 0.1344451538554789 0.1231573433965116 0.0100001057689818 0.02608856461948328 0.01000036777533886 0.05240154309470624 0.01701856262822377 0.02916678276405538 0.01000000268727197 0.03625229671967922 0.01006670441785218 0.01316706148244196 0.01000009233094845 0.002333466901676129 0.3789598593529813 0.2664499366865423 0.01000001076950907 0.01626186229841275 0.09697810485527247 0.1110727681007844 0.009999999999999787 0.01273298991448435 -0.1803201216495383 -0.1054187654706842 -0.1870093596817806 -0.06942831941581656 -0.1208955907404824 0.2384178639620064 -0.08178658451841098 0.01000005424229422 0.01000000000000001 0.01000000000000001 0.4151384578700784 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1380381862075983 0.01000000000000001 0.01000003521110793 0.01708761724329511 0.01000001321245902 0.01000000000000001 0.01000007059158392 0.2921115222801922 0.01000000698704118 0.09494417247246789 0.01000000000000001 -0.4601777045196003 -0.2945639579858091 0.05258744926573935 -1.454434290535295 0.2247575027476746 -0.1624233515755278 0.06516053391766218 -0.4870893270902478 0.175681658286667 -0.02653952149575197 0.183598806255707 0.002079613637840236 -0.01485738001581184 0.1039916068561611 -1.106282597493332 0.116473155700475 0.423776159344226 0.2059426149639553 +0.3203789794685529 -0.4031248522770245 -0.0009562013664683455 0.01932348388293859 0.854063105286202 0.9738642850314614 0.398013077167831 -0.005883046178770002 0.01469107802920711 0.2113605877024587 0.7810120797263362 0.7132515245129056 0.7132515245129056 0.3236910017685473 0.3236910017685473 -0.2218436240557944 -0.6253671809651231 -0.1749472691710769 -0.09875559530973721 2.533233746028161 2.523434329015034 -0.03676206353505584 0.001413902396849132 1.427300596524423 0.06151508208948986 -0.8955847263782843 2.765390192873761 -0.2192337700739087 -1.553131161402904 0.4306604827758735 0.7529736309628272 0.5995977986316237 -2.066405766774027 6.608519870282743 6.608519870282743 -1.073650356687569 -1.073650356687569 -1.209973323964164 0.7578063315826942 1.987987667424205 -0.04612182738264092 -0.6419217523779164 -0.5286137104720523 1.125542548692044 -0.04474595817586913 -0.03023417966146447 -0.09829449287176395 -0.2057365679223047 -0.07682037625830906 -0.082923988012789 0.1403046986674426 -0.0673476621737783 0.01093803354225198 0.02960766638172529 0.01059195019344994 0.01082003905667817 0.009999999999999787 0.02640546071736338 0.4261039692689623 0.416681493745819 0.01000013006666922 0.03256838212489077 0.01016325322037925 0.01011781375650944 0.01001778451974022 0.01731519885799759 0.1385520710124988 0.121293038611292 0.01000004751340189 0.02727076704696119 0.01000020256880729 0.05209667800512507 0.01592756064471157 0.02920418517669043 0.01000001214736379 0.03596702439588739 0.01003302853998278 0.01327245913907227 0.01000006740102188 0.003083965184831783 0.3111072038976599 0.2438943258761905 0.01000000444214599 0.01684981892521042 0.09034266028743332 0.1103035417851466 0.009999999999999787 0.01418896559494742 -0.436099698274949 -0.07716554306078177 -0.1527623305622992 -0.007486602249198782 -0.1672486990782781 0.4652283852614412 -0.08429006759525792 0.01000001647226068 0.01000000000000001 0.01000000000000001 0.3107399595267915 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1488860698271048 0.01000000000000001 0.01000004415324596 0.01328749705245968 0.01000002704173519 0.01000000000000001 0.01000002405026656 0.09406293735789528 0.01000000000000001 0.07260133738527008 0.01000000000000001 -0.3848192550594027 -0.2466185216934349 0.01740870951603073 -5.459007712042615 0.2283396638240626 -0.147598808695299 0.05846959150626962 0.00172869268775483 0.1629007148726334 -0.04436181546036812 -0.2126529435757505 -0.1230602327886766 0.06713039339896022 0.1097987147624877 -5.651649243954514 0.03399651988103491 -0.7209503583998003 0.2046070551354112 +0.3273437398917823 -0.3923372211246332 -0.0004973831180503652 0.01296691074206047 0.8732458807588079 0.9721751100998439 0.386033561676272 -0.002906585690134023 0.02002096493722316 0.2159656996864161 0.7665167589265174 0.7607044905876843 0.7607044905876843 0.3203202406988312 0.3203202406988312 -0.2302124954191025 -0.6183754835511832 -0.1620863843744638 -0.1000736256550421 2.527054969197153 2.518461044077259 -0.02966443391499718 0.001167013218858592 1.608662051684376 0.06944240089962239 -0.9302095964045325 2.740690632543697 -0.2711852294686095 -1.815779394254978 0.4232017055921675 0.7768954770329883 0.7998752497890771 -2.030590225290443 7.034531771635912 7.034531771635912 0.08501101085874252 0.08501101085874296 -1.198848220928946 1.247777718638585 1.662386221951814 -0.2412624695309082 -1.146642668418326 -0.8294008762905274 0.9163709479327169 -0.022647926183887 -0.06059450270440347 -0.1053334092653193 -0.2055581788454832 -0.06226508567664313 -0.1024194094765565 0.1923925799189035 -0.02556253118235707 0.01045294110245498 0.02707154084052021 0.0992739987281217 0.01807278232517184 0.009999999999999787 0.02667909843677974 0.4156764689762915 0.3897888144265642 0.01000005855813768 0.03410465877239854 0.01007873004339821 0.009207814192397912 0.0100085702678494 0.01747608868560224 0.2061082900153641 0.1331879329103742 0.01000001791750416 0.02839984059680045 0.01000011920110788 0.05155112484430946 0.01458432612793548 0.02702246587301049 0.01000001586642973 0.03563752534540665 0.01001592277310959 0.01345318950031871 0.01000004608265437 0.003792891022802891 0.281368779026903 0.2040757526406853 0.009999999999999787 0.01668664224711147 0.1037186391293354 0.1119244030852542 0.009999999999999787 0.01541536560926637 0.02529811364089452 -0.1938988191461106 -0.2536897162973165 0.0144673251450107 -0.213236207480982 0.3946823586491192 0.3980561751166899 0.01000001858966704 0.3601896365056652 0.01000000000000001 0.4685153375572817 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.438358759066542 0.01000000000000001 0.01000003942329564 0.01332624762754575 0.01000001286846708 0.01000000000000001 0.01000002806256584 0.3418987695221065 0.01000000000000001 0.1604197350098027 0.01000000000000001 -0.3523679712644325 2.015881663269204 0.06720600311110549 -2.100927485508622 0.2076993973983145 -0.1066882550303244 -0.01184900556313402 3.57869946699919 0.1666945283518758 -0.1069099671211645 -0.3570714336146851 0.003964143563223698 -0.007765537217793946 0.09225132820424913 -5.149408317982721 -0.04231815746483553 1.178154440218807 0.1571743085993183 +0.3343085003150117 -0.3815779803062549 0 0.006359962401108366 0.892205578335354 0.9700122027731819 0.3737108923951569 0 0.02550365577217617 0.2235760077366384 0.7536390229734087 0.8114684894265207 0.8114684894265207 0.324586936318175 0.324586936318175 -0.2386212498125841 -0.6080162000402711 -0.152386605636023 -0.1008472700848131 2.517062825013944 2.512857819473362 -0.02394509717485338 0.001147219831816315 1.419148030660006 0.07258936671325866 -0.9674824471514463 2.701331163558203 -0.3552707236111337 -1.651671173887599 0.4105952416897347 0.7968527933910838 1.462569640795185 -1.601900242266194 7.5593157520969 7.5593157520969 1.119521044749527 1.119521044749527 -1.221426131452406 1.724842063566951 1.080244725480482 0.1102039849331984 -1.73696799100674 -0.7095071304475344 0.7297520846527723 0.02046684948361133 -0.001953636701200079 -0.1322254358571739 -0.2236784889423014 -0.04637074765099625 -0.1267095197725938 0.2222854778214978 0.09595785989598138 0.01022435431045476 0.02457518670953229 0.2871254515724271 0.03453431944869978 0.009999999999999787 0.02742569966449127 0.4613688538923459 0.3896810228763155 0.01000002519298659 0.03539024381380029 0.01003898382910373 0.008729598785322423 0.01000423918461024 0.01715599100121867 0.352799565026769 0.1734366789356621 0.01000000401993262 0.02966755156661716 0.01000007750874232 0.05068254258367366 0.01396454984184725 0.02502124082685864 0.01000001010315366 0.03568128776058765 0.0100078828185528 0.01326829148324205 0.01000003818867956 0.00434773891782303 0.3510651746588573 0.1809862718297572 0.009999999999999787 0.01679713755674372 0.1494945936072893 0.1265998572167124 0.009999999999999787 0.01651216939385414 0.4866959255567381 -0.3106320952314395 -0.3546171020323338 0.03642125253922013 -0.2592237158836859 0.3241363320367971 0.8804024178286379 0.01000002070707351 0.7103792812005009 0.01000000000000001 0.6262907155877721 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.7278314483059795 0.01000000000000001 0.01000003469334565 0.01336499820263204 0.01000000000000001 0.01000000000000001 0.010000032074865 0.5897346016863175 0.01000000000000001 0.2482381326343349 0.01000000000000001 -0.3733985003142031 2.397777924281034 0.1532248696036318 2.232538340334479 0.1563510839232331 -0.02360770755017705 -0.07965074079480237 8.143818299012249 0.2027118243888117 -0.1371228237907607 -0.1608222280578646 -0.0158745026378747 -0.03786342005924595 0.06555370600618407 -0.8477245120820593 0.1125841441650776 3.027805388900565 0.1673922130283307 +0.3412732607382411 -0.3670096942548957 0.0006063720507687442 -0.0007789945021108302 0.9109868205993945 0.9676259944724022 0.3564431559667063 0.00286653031827333 0.03140232694249079 0.2311406162113103 0.7384961377908055 0.8691450442626092 0.8691450442626092 0.3388404412115182 0.3388404412115182 -0.2456955961653686 -0.5932094680353164 -0.1421100221664213 -0.09548027692571281 2.503094836843346 2.508869997337774 -0.01887689120005565 0.001459450541191565 2.516194589834564 0.1008542513484101 -1.074482939584334 2.685129811618171 -0.3490519471136153 -3.016257099388684 0.4063870219309274 0.8924022402481255 0.9689577703258756 -2.47986567363054 8.945653935340827 8.945653935340827 2.916613519789006 2.916613519788997 -0.8715274212031607 2.458820768020243 1.779989963963319 1.365477318214819 -2.22804061967514 -0.4886254664554519 0.6885299775841078 0.02893422479889463 0.04366924010509488 0.08404549493473024 0.1793287303291895 -0.0543902211905456 -0.06169840460998088 0.158492833410619 0.00794385790081531 0.01019275854920654 0.02147283710252967 0.2794206758611399 0.1094523187416874 0.009999999999999787 0.027123627942649 0.4565341301223595 0.4086229219122806 0.01000002046094028 0.04129167165007441 0.01003349094152339 0.002208909733582676 0.01000364055978764 0.01763672004889516 0.3473308440689671 0.3773517135407767 0.01000000208042762 0.03482563641297665 0.01000006789906349 0.04904192198841306 0.01340989204985199 0.02596302681878671 0.01390407278241534 0.03716591040052775 0.01000677145440454 0.01433159381251814 0.01000003258750226 0.006346966806175303 0.3194717032869749 0.2558508789080909 0.009999999999999787 0.01862953678382162 0.139514355382266 0.129861090879984 0.009999999999999787 0.01605094416305697 0.07378598646472934 -0.06160243171099822 -0.01150535548381748 -0.00478949467335188 0.1331164342131258 0.04158262230438703 -0.3498847090884142 0.01011218527768498 0.2476041270085925 0.01000000000000001 0.4333938420949675 0.01000000840035431 0.01001948983685141 0.01000211482620483 0.3232149842457597 0.01000000000000001 0.01000004340683902 0.0119822702279877 0.02391330091246036 0.01000393886053075 0.01000001831175945 0.1805325836955641 0.01000000000000001 0.1054655763653873 0.01000000000000001 -0.2983675543944159 -2.542063603013922 0.2990080710234389 0.5026258418740547 -2.638797126966161 6.042169273722015 0.2441531596128908 -7.897345978077432 -8.38570266869092 -0.5772894343583068 -0.1856359401239308 0.7824009783749462 -0.1292631995380543 -0.3663768771243257 -13.37144545980547 0.320746372859343 -0.2709587275673929 4.10254386070639 +0.3482380211614705 -0.3499843024496712 0.001395349686882152 -0.008494841914417961 0.9295140014078687 0.9648840322779821 0.3357448177734024 0.005574826693029777 0.03787021990586181 0.2406847030624566 0.7228101796918667 0.9352772623905987 0.9352772623905987 0.3644213661256335 0.3644213661256335 -0.2516176639257948 -0.5747165142871431 -0.1288568334715698 -0.08273926447614333 2.48666856933871 2.505313376409124 -0.01487103884557062 0.0009894701760009283 2.12473030617231 0.1250263707868786 -1.133142275787578 2.628374897979074 -0.4574289808999854 -2.636802479615405 0.3651567961746673 0.9603104741951216 2.030996479954697 -1.757849013893955 9.987443290730276 9.987443290730276 4.372301997025883 4.372301997025883 -0.8905379106495424 2.783377180136629 1.935006424065637 2.227726725736344 -2.442887271692327 -0.5856834292066149 0.4246867110413133 -0.2041528149561085 0.01496276043213518 0.07187125872806899 0.1469360122532803 -0.02885599005343975 -0.04591083163608856 0.1707146923658138 -0.04854416256642713 0.01011847147874834 0.01852520131625957 0.2596635490691321 0.09423466848283502 0.009999999999999787 0.02830218559804187 0.4439954218392028 0.4006167145745692 0.01000000933719036 0.03528238502074377 0.01002057823694225 0.01515944607198172 0.01000223335114958 0.01905495929702861 0.3332572982764819 0.3336506501942882 0.009999999999999787 0.02643400742897528 0.01000004530933918 0.04657244192059729 0.01210171038274543 0.02576221347648966 0.0229924177288372 0.04409071059799441 0.01000415892318651 0.01388886226568742 0.0100000194206662 0.006725982790007023 0.2345037998401263 0.1882639261444923 0.009999999999999787 0.02094141695587393 0.1143356473591508 0.1182654125057416 0.009999999999999787 0.02236698810076909 -0.2912830174432942 0.08915989306897321 0.01021110386438562 0.1725852653875612 -0.07384721368166153 0.3977927981419696 -0.2637082531264214 0.01000001624491542 0.2080828024447653 0.01000000000000001 0.405418830297599 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2936304034647526 0.01000000000000001 0.01000000930493616 0.01000000000000001 0.03782659172176694 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.06143655912348545 0.01000000000000001 -0.695232087519524 -2.640172576366534 -0.2042880136972626 -10.59981608508905 4.935865889731112 -9.130020574134491 -0.05049592814187846 -5.560950369416981 15.09044386676547 0.1542651449433404 -0.2237290181355532 1.023124160050566 0.1827867918910323 1.11130764169538 -11.1653129965125 0.114876306470382 -2.815940675906568 -6.289691670653784 +0.3552027815846999 -0.3375596283411135 0.002660912678425831 -0.01569838771709353 0.947591397679314 0.9612547579353823 0.3198785026903925 0.007208384251021993 0.04402549420799096 0.2593105089383774 0.7146147751223833 1.007765703625532 1.007765703625532 0.4030253037796996 0.4030253037796996 -0.2578676107194031 -0.5542327765191881 -0.1149925265648148 -0.05857256859264659 2.468672103301477 2.500340472662276 -0.01165063812866141 -0.005923372180192565 1.463230825859308 0.2421971783839543 -0.9275523634933593 2.565557546975886 -0.5810374373679317 -1.937358046895293 0.09790270102231835 0.8011027497833647 3.252195596476902 -0.6372141278571222 10.73068424084614 10.73068424084596 6.700605655212167 6.700605655212161 -0.904061749955446 3.099388848703086 2.099989828571792 4.917490401342304 -2.727718696261017 -0.8498476358205573 0.4606919833580232 -1.627476151122551 0.001414076623879712 0.07069429350878931 0.1200759125891353 -0.008466411679305619 -0.04368443720028825 0.1901173197182118 -0.06676019482399065 0.01005714284866599 0.01556301411788663 0.2279992924530783 0.07607056379210286 0.009999999999999787 0.02958213600670723 0.4042736791588131 0.3292901267120003 0.01000000054337535 0.04334727441759378 0.01000991991681621 0.00383955324636176 0.01000107202290712 0.01939184807366523 0.3046569208868419 0.2976977189779273 0.009999999999999787 0.03768619039232224 0.01000002562695235 0.04345009932851918 0.01101662614487564 0.02614859051801544 0.04934853954458296 0.06520265647234469 0.01000200319079614 0.01309375008552882 0.01000000955166902 0.0093971308470846 0.1450505283260335 0.117647174448857 0.01224755453324011 0.02541240589118798 0.08437650821870069 0.08578537292359467 0.009999999999999787 0.02078573901311875 0.1575403720479601 0.04293528591925433 -0.01326139970121842 0.0008160982506359993 0.004337315353127424 0.1656353314452749 -0.0419177285425017 0.01000002497361452 0.1484129654918217 0.01000000000000001 0.2518564530303651 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2170529759074797 0.01000000000000001 0.01000000562258663 0.01000000000000001 0.109038303772794 0.01000000000000001 0.01000000197795026 0.01000000000000001 0.01799531992413506 0.03571827596156441 0.01000000000000001 -0.2670645648385561 -2.545658561403444 0.3729371246402571 -10.20510248171877 -1.102311448293948 2.750187660764438 0.1269905298946672 -5.061548081479452 -5.701922276429958 -0.787822962729989 0.3167779150556416 4.717513692374877 -0.2684714809736695 -0.004465871874462367 -8.980791319270056 1.147358089922605 -6.035547447671064 3.040722384888101 +0.3621675420079293 -0.3293223152078144 0.004822036449367406 -0.02130550762296846 0.9652903939225221 0.9568422169303763 0.3085077443959561 0.006854531095179972 0.04894374237291066 0.2850753400823303 0.7133536285314825 1.083390376631768 1.083390376631768 0.4575821396402633 0.4575821396402633 -0.2642089371273597 -0.5315343517461475 -0.09885664458436727 -0.01137831202547268 2.448634555425308 2.493370761140942 -0.009002522146571934 -0.01954283723871875 0.9222880911011138 0.3821955266465631 -0.6747127535139774 2.519713578637891 -0.6823517678500437 -1.345858781476577 -0.2055478923042364 0.6050861557624825 4.081038357743684 0.2333919067692651 10.88804333459147 10.88804333459147 8.953334864467926 8.953334864467926 -0.9167784727992707 3.419346508720333 2.587311725650646 8.840319057507655 -3.029004155809955 -1.159086576807186 0.2603494761517195 -2.130026009409244 0.07154812499605878 0.06136434939509394 0.09382891936788607 -0.0228548178884771 -0.0277107067950495 0.1639911679904089 -0.04153847299210245 0.01002831170267005 0.01324953740687151 0.1839744957691734 0.0591951403082418 0.009999999999999787 0.03072642072172815 0.3234797865759753 0.253968331382612 0.009999999999999787 0.04106773364350413 0.01000490962544109 0.00987979816281026 0.01000052641182858 0.02054185457478486 0.2573929948522151 0.2589911219350922 0.009999999999999787 0.03277783761474318 0.01000001442404219 0.03926265907544213 0.01050370990706018 0.02992539642772529 0.09587632004217195 0.105319967056428 0.0100009909951293 0.01213608524256493 0.01000000679063406 0.01139695861496604 0.08599732096235702 0.06500723299035771 0.01750081964768491 0.03662192998313074 0.05506119364698403 0.04081629849262258 0.009999999999999787 0.02579120491985609 0.6063637615392147 -0.003289321230464326 -0.03673390326682247 -0.1709530688862893 0.08252184438791621 -0.06652213525141992 0.1798727960414181 0.01000003370231362 0.08874312853887834 0.01000000000000001 0.09829407576313109 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1404755483502068 0.01000000000000001 0.0100000019402372 0.01000000000000001 0.1802500158238211 0.01000000000000001 0.01000000552106151 0.01000000000000001 0.02599064954691965 0.01000000000000001 0.01000000085749975 -0.5089484401636039 -2.27011499253159 -0.243245762569965 -11.74707951584641 1.965360217672229 -4.144884923480984 0.182998887317113 -6.351692809808636 10.44978582715991 -0.151588207204738 0.7498662419494966 6.480703044055116 0.1361081846992573 0.9185313658227837 -6.003093725500717 2.049913478738087 -6.402290841885046 -4.398251161411229 +0.3691323024311587 -0.3232952613150535 0.00765149536386911 -0.02616371368347137 0.9827797998084833 0.9519043204705424 0.2995870628037007 0.005271334642407588 0.05342595773162895 0.3142216405465219 0.7147121564691066 1.158333282007499 1.158333282007499 0.519969445159953 0.519969445159953 -0.2706437881441897 -0.5065176695632221 -0.08033047012942829 0.05139277682135068 2.426844272070052 2.484206609375257 -0.007038406936360708 -0.03485901831721661 0.8409540796746913 0.4220315591181278 -0.7345336754104803 2.50457999843564 -0.7305763851391238 -1.251191041947434 -0.2337296866705687 0.6948418352897985 4.23635560264943 0.09395301649648413 10.58071565756388 10.58071565756389 8.79407165212948 8.794071652129494 -0.9315908675538329 3.761982049407257 2.65570059865075 8.832436219435579 -3.210358138202098 -1.469544151833194 0.2744032734728341 -2.314541275386113 0.1461041806136358 0.05075256460010502 0.07034165709715756 -0.04151416535368702 -0.00293834921569136 0.1280236253319593 -0.0080244716115212 0.01001367494216954 0.01133893604670311 0.1417747301848453 0.04409588542094589 0.009999999999999787 0.03132792263435213 0.2593225155649859 0.18985534202834 0.009999999999999787 0.04433076796238389 0.01000236505654284 0.004989810954512119 0.01000024932369126 0.0215585621505463 0.209993025102249 0.2150374242227566 0.009999999999999787 0.03973365606152468 0.01000000813294122 0.03639877229408439 0.01024305080463339 0.03279348382670344 0.127165846340795 0.1333591871821809 0.01000047673683779 0.01230046719859201 0.01000000489012765 0.01423082978945001 0.05043649771055581 0.03892096475439821 0.03729561749594623 0.05734428604977149 0.0331277271623569 0.01789796084943118 0.01340166111981578 0.02824680907108856 0.37000008156145 0.008088172315415587 -0.0358904459674414 -0.08413514178114062 0.1349712240828451 -0.005872727666101474 0.1105033844280782 0.0100000560186474 0.07563952196407864 0.01000000000000001 0.1870590677125002 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1273723202108483 0.01000000000000001 0.0100000025562379 0.01000000000000001 0.1535034552678421 0.01000000000000001 0.0100000011097936 0.01000000000000001 0.08316483706491273 0.01000000000000001 0.02211614021090769 -0.1346811888084966 -2.060038516112899 0.2466836302156407 -6.473744383424955 -0.4267716261086164 1.29053076509717 0.1068232197462706 -6.051789288694624 -4.112257978364855 -0.5158638315219014 0.1354048477049053 1.861976255945585 -0.02179169625683448 0.09896494282667348 -1.96892057959297 3.684865208234069 -1.007428685300639 3.084124847068298 +0.3760970628543882 -0.3171553248193701 0.0105853961482314 -0.03173451303424635 1.000206371799116 0.9467358116214797 0.290586319407931 0.003812648169698285 0.05880106881759328 0.3433578544641449 0.7137879952147692 1.230052580379877 1.230052580379877 0.5777429910088432 0.5777429910088432 -0.2771930467283301 -0.4791660072407535 -0.06293605608356723 0.1067419500772058 2.404165611092669 2.472941755773632 -0.005587827295533643 -0.05242925651617192 0.954703449566674 0.4121870449520206 -0.8793409810396073 2.501670202695168 -0.748574401570496 -1.368849322025966 -0.1697936172115284 0.8614900154763425 4.078141905928382 -0.4221049238975354 9.962352626667153 9.962352626667153 7.628448036959782 7.628448036959782 -0.9496317399176188 4.089859203391835 2.262315504575972 6.709042419187632 -3.284110596851797 -1.76232393410439 0.1128826465221664 -2.777300856869079 0.164388211023569 0.04412052468263461 0.05125790707774192 -0.0410836347124337 0.02684315351536437 0.1095423688020607 0.006869103509133723 0.01000680647020236 0.01005044608615613 0.1101888875663852 0.03057984683566151 0.009999999999999787 0.03180448568950123 0.2500598541286001 0.1664367851280728 0.009999999999999787 0.04350267396588858 0.01000116988654209 0.007656436438981729 0.01000011898566511 0.02200010362033389 0.1742312535113721 0.1777328252683299 0.009999999999999787 0.03595150689736881 0.0100000055023699 0.03423518311807161 0.01012036522241022 0.03267057357785541 0.1317125254705291 0.1353090488550341 0.01000023397843641 0.01276738344949546 0.01000000165900161 0.01561324419505805 0.03075287999438681 0.03087984622784123 0.07463946765984009 0.08494363172191832 0.0216675120604175 0.01524280757386132 0.0213300131600529 0.0406240197817076 0.1336364015836854 0.01946566586129539 -0.03504698866806033 0.002682785324008008 0.1874206037777741 0.05477667991921686 0.04113397281473818 0.01000007833498118 0.06253591538927916 0.01000000000000001 0.2758240596618695 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1142690920714897 0.01000000000000001 0.0100000031722387 0.01000000000000001 0.1267568947118629 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1403390245829058 0.01000000179610794 0.0342322795643154 -0.3303029845296392 -1.81547873633774 -0.2791231940997259 -0.06126981587952809 0.7905521649130246 -1.974928677008215 0.0178342381418212 -4.442338186165789 7.366271980257276 0.04950919246973104 -0.1090283836058995 -1.011114870207653 0.2229839078028129 0.5017311659106927 -0.821250097537856 4.024709217992956 -0.5835165735848165 -1.54916347935531 +0.3830618232776176 -0.3105866298303956 0.01342159550468436 -0.03824053007070471 1.017584690022515 0.9414512232873533 0.2811494045361589 0.002791968699699332 0.06528069719403051 0.3718211951438062 0.7101480280745105 1.297275344586919 1.297275344586919 0.6291064137683189 0.6291064137683189 -0.2839104367732119 -0.4496712557308213 -0.04846097670169591 0.1512043241272378 2.381355477230234 2.459653710003945 -0.004453425398968758 -0.07286771133514236 0.9137921674436571 0.400662319565785 -0.9949109211628961 2.488383143741967 -0.7672487329683542 -1.320707691745018 -0.11782754430901 1.005576828902464 4.115297550560204 -0.6238027223654206 9.331778832411961 9.331778832412015 7.096708501422869 7.096708501422871 -0.9855732753738558 4.371409331810509 1.908427387729509 6.117200906879392 -3.250829902822831 -2.052732421532371 0.1809397993030193 -3.033967886022385 0.151906805445007 0.03958273897819353 0.03630553908812351 -0.03596601422951418 0.05768826332654875 0.09835933416472731 0.02078868067687267 0.01281636385355789 0.009888838075722273 0.0903690157398862 0.01934206904041336 0.009999999999999787 0.03232072783317763 0.2456398281670098 0.1636552069178716 0.01000000167559545 0.04472388558769147 0.01891347964791068 0.007353697095786149 0.01000005285808081 0.02215145267786056 0.1478018386643409 0.1509869543788636 0.009999999999999787 0.04094824896310012 0.01000000490713804 0.03238656921844774 0.01394675729411432 0.0341257257989791 0.1159524457436243 0.1223125204075233 0.01000011013601743 0.01309281584412325 0.009999999999999787 0.0174040680602956 0.02031572217120114 0.02644313584422253 0.1123561000144901 0.1138531076211575 0.01572498046092896 0.01220832658831261 0.05459577456678311 0.07064089454325373 0.05908532494442231 0.01852173645400912 -0.02832362253687754 -0.02724031513404279 0.2074488627911055 0.04081273323178092 0.1243340741354894 0.02001323595854154 0.06872440779546907 0.01000000000000001 0.2051470641055524 0.01000001382780824 0.04194220072951749 0.01000000000000001 0.1070762949189231 0.01000000000000001 0.01000000534197798 0.02385953403609786 0.06837844794949755 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1723635950480218 0.01000000231314946 0.1354577833913885 0.1990954642428659 -1.391106560379898 0.2572918735210213 -1.041580345179783 -0.2102612726053293 1.198979090677492 0.02463037680194216 -3.285369898465484 -2.886112467041445 -0.4185004317335182 0.4990169971232392 -2.862041372008123 -0.04525333687356645 0.1037469099007952 -0.4804176985766283 4.253376614962781 -0.3256687313236011 8.601519137146349 +0.390026583700847 -0.3046741690794144 0.01614423502306828 -0.0456765082480679 1.034864135894472 0.9360720677857297 0.2724729130780745 0.002247659849702988 0.07289710524284843 0.4009593004763885 0.7050895983865613 1.359906737705384 1.359906737705384 0.6762574239509882 0.6762574239509882 -0.2910085286364659 -0.4183919594160153 -0.03615644446632915 0.1927655552231577 2.359095053722224 2.444358452758234 -0.003512224291103205 -0.09388534525558079 0.7662592825845085 0.3795772116880003 -1.14638854172641 2.473278026816516 -0.7757303012023757 -1.150486200368845 -0.03299907122507051 1.187927567752379 4.271907265265282 -0.8294305062235181 8.64389490248098 8.64389490248098 6.418812006190249 6.418812006190249 -1.058954041116773 4.602308935019728 1.639021373935079 5.875950459695412 -3.126255819075829 -2.338714607735836 0.05740312476919485 -2.943617877491627 0.1281916771581972 0.03569538986240328 0.02527616895678797 -0.03719026306571038 0.08657567785200504 0.08667299422115926 0.04724704183030415 0.01938056924431475 0.01164252465836846 0.08213553528301665 0.01148731344718001 0.009999999999999787 0.03301936979513531 0.2152332852292806 0.1476922085315597 0.01000001267105244 0.04377216278037199 0.0394285844869624 0.01475999210923007 0.01000002199190231 0.02232930847636094 0.1288673145215458 0.131309409478753 0.009999999999999787 0.0381688502078954 0.01000000577724691 0.03066025706883257 0.02297096663970155 0.03923338687468725 0.08012354892586426 0.0934770870766215 0.01000005169379747 0.01331099827168636 0.009999999999999787 0.0183291129582952 0.01514548875884003 0.02380309071396791 0.1456597937895436 0.1438629955347674 0.01284560059770179 0.01017980647608141 0.1177616767049998 0.1386074960303381 -0.01546575169484093 0.01757780704672296 -0.02160025640569485 -0.05716341559209359 0.2274771218044368 0.02684878654434519 0.2075341754562406 0.03002639358210202 0.07491290020165919 0.01000000000000001 0.1344700685492355 0.01000003003833738 0.07388440321032586 0.01000000000000001 0.09988349776635652 0.01000000000000001 0.01000000751171737 0.03771907612335113 0.0100000011871324 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2043881655131379 0.01000000283019087 0.2366832872184619 0.2196928449498683 -0.8440103738149008 -0.2267452781140235 -3.846445292361411 0.1665725024145857 0.2387969613976777 0.02544591157533284 -2.412598307202823 5.133276057112723 0.08462935238842538 0.9398257429907743 -5.559413424664347 0.1921860174528783 0.2531141527342643 -0.3053198589675419 4.340703436848836 -0.2946449363142297 9.348476763109508 +0.3969913441240764 -0.2997078488688105 0.01869129953305748 -0.05425763861579203 1.052061544868307 0.9307218978378318 0.2649120340233373 0.002372316525744544 0.08187788257989315 0.4309765671396493 0.6976623397505866 1.417418171557934 1.417418171557934 0.7173691682037826 0.7173691682037826 -0.2987165359965855 -0.3856450950941088 -0.02564607384646322 0.2330376620034862 2.337926551227994 2.427178639140918 -0.002794747334796011 -0.1138147974653734 0.6864995516225991 0.3446768222209498 -1.319346182724522 2.467176834957386 -0.753745455725259 -1.050791075887141 0.07536249012883101 1.390415097764314 4.30777810869586 -1.368127004127031 7.849870469074018 7.849870469074048 5.339169339351422 5.339169339351341 -1.155588688807937 4.792812236090506 1.370398524334407 5.73687358449804 -2.941599297892369 -2.585675826482234 0.120252668330552 -2.766125705283646 0.0920612033229502 0.03278732672411433 0.01792860714897193 -0.04187318540257801 0.1169983338488101 0.06869544076836753 0.08030461999887972 0.03215206432843409 0.01599700010681548 0.07661376994561575 0.006954064198154697 0.009999999999999787 0.0335721927403565 0.1883671267930289 0.125101908291418 0.01000001966693143 0.04344636510496036 0.06186021674295805 0.02008617072479568 0.01000000640336074 0.02246250751389445 0.1161270811487061 0.1173368837123085 0.009999999999999787 0.0417884721231081 0.01000001050327937 0.02983973646203175 0.03571219352068988 0.04705535386227044 0.04707033027410201 0.06387593069935527 0.01000002185237925 0.01396978617077815 0.009999999999999787 0.01928794944749068 0.012500481655767 0.02199165882104515 0.1802281252624378 0.1744715913165247 0.01137802800461829 0.0086016505620643 0.2034308531723825 0.2144203425477516 -0.1251941694474826 0.02147195515012079 -0.009625202763107021 -0.06875772101468941 0.2811610834416388 -0.05037827682912277 0.2519240507026398 0.0571789057751233 0.06617436637877794 0.01000000000000001 0.1582247558277851 0.01000002294911517 0.09586677980839653 0.01000000000000001 0.09930644639895647 0.01000000000000001 0.01000002109561482 0.0574642595755972 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2501952616278776 0.01000000034082715 0.398772880528043 0.9671651942266976 -0.4889177216709981 0.2290337963118615 -2.471434410301357 -0.1805499068147355 1.111124831218571 0.01068601313035195 -1.606087697679969 -1.994893025975507 -0.2005927293971567 1.326088942248821 -3.124942729860336 0.05701888099481078 0.07085693450138766 -0.2253786580887636 4.489533766811449 -0.1850702774169994 12.10284786218296 +0.4039561045473058 -0.2947406056387383 0.02084567162676665 -0.06407635017903202 1.069259260141713 0.9256685700987495 0.2574166753897349 0.003388898649606453 0.09225688989253511 0.4604064250603153 0.6851303561522881 1.468956185215308 1.468956185215308 0.7499655638203366 0.7499655638203366 -0.3071208150484348 -0.3517483025398409 -0.01718915492373618 0.273350102901333 2.318271680612206 2.408466082690779 -0.00223240999245089 -0.1322323343565159 0.7665243368080947 0.2668116914321432 -1.501772250654915 2.473365983775956 -0.6904891182212183 -1.131671361946172 0.2231261631334416 1.589443166455408 4.103227021564457 -2.295302884384435 6.928568076510938 6.928568076510938 3.973545229223672 3.973545229223672 -1.258895359814373 4.932522226646591 1.049352362411882 5.887532801416878 -2.691601419945969 -2.77885690687826 0.01285324665950593 -2.509456405523286 0.04261055427645477 0.03111036653188304 0.0140781623757249 -0.04780696922821637 0.1516309534706526 0.03999457343658541 0.1154131818581843 0.05181747872517795 0.02422913439783114 0.06951125075319986 0.004242871155051287 0.009999999999999787 0.03403028701627342 0.1814946250328884 0.1156224753340638 0.01000001919818461 0.04236568124536078 0.08328885345615689 0.02773647789558753 0.009999999999999787 0.0224486637674004 0.1086365113300047 0.1088491099993556 0.009999999999999787 0.0396186304209678 0.01000001992367849 0.02953286461251281 0.05166200577559943 0.05798040832757767 0.02896013004501929 0.04738383759155296 0.01000000770519138 0.01494139720013843 0.009999999999999787 0.01999353216148414 0.01124009612671495 0.02051702551306311 0.2179934841251518 0.206966639812344 0.01068293310169821 0.007232286774680574 0.3110474366724985 0.3027496551220583 -0.2349225872001243 0.02536610325351851 0.002349850879480697 -0.08035202643728523 0.3348450450788409 -0.1276053402025907 0.2963139259490389 0.08433141796814481 0.0574358325558969 0.01000000000000001 0.1819794431063346 0.01000001585989307 0.1178491564064673 0.01000000000000001 0.09872939503155642 0.01000000000000001 0.01000003467951227 0.07720944302784327 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.2960023577426174 0.01000000000000001 0.5608624738376243 1.333201022383744 -0.3207888714728488 -0.2539475546213841 -0.08152977844004512 -0.05020054885855487 0.9061935158740732 -0.0167788815466647 -0.8375973824605302 3.470774187214851 0.2321286850172461 1.830908074126437 -1.795015951454465 0.2820162811254556 0.1803898206293589 -0.2086048064906058 4.882417034857144 -0.2346882073307602 12.94270407298889 +0.4109208649705352 -0.2885080307165162 0.02223617149113721 -0.07502908513957962 1.086547059499846 0.9212244980945816 0.2485382547932362 0.00553900605707236 0.1038164927383538 0.4873693099970811 0.6647849924782929 1.513686940890826 1.513686940890826 0.772161639177575 0.772161639177575 -0.3162248004157151 -0.3170961003669843 -0.01075099785637068 0.3149197635250567 2.300558047281123 2.38863067320618 -0.001805342398076171 -0.1489686451251688 1.029378393289614 0.1260272950173826 -1.616960025308663 2.490799741808574 -0.5804357926939554 -1.428424740112752 0.3869742768626363 1.701014594343264 3.625981577623779 -3.507582741375176 5.910398832921574 5.910398832921594 2.462369266326282 2.46236926632628 -1.351963356434169 5.002870273388174 0.8118650451215559 5.990986819194079 -2.389327780649503 -2.896810215673629 0.08326933708138018 -2.275906512185497 -0.01068322182481385 0.02831538238709808 0.01243627359933663 -0.05167895662504041 0.1872232330736621 0.00525719272897307 0.150726207886601 0.07422949955932578 0.03524826262520797 0.06703554882111984 0.002698263794196798 0.009999999999999787 0.03417892234293562 0.1877881025027426 0.1198963721873341 0.01000001533504369 0.04179328341376198 0.1052778549380835 0.03428601860729596 0.009999999999999787 0.02235854103015988 0.1044852984295646 0.1046593563907656 0.009999999999999787 0.04206698626688876 0.01000002923550403 0.03016426554644669 0.06784283320554785 0.0724017203737839 0.01940095938963182 0.03822339279243847 0.01000000022835179 0.01629755756928031 0.009999999999999787 0.02097781929076614 0.01059905848871123 0.01908507892412281 0.2573977465754695 0.2422461099183164 0.01032962564422357 0.005986466166302051 0.4008120228327137 0.3818539257601445 -0.2693533346611771 0.006454531788746998 0.00745898302988679 -0.05881609583576708 0.3619876765448155 -0.1758663755378503 0.3249440413696325 0.1115964393706866 0.07000234856229703 0.01000000000000001 0.2101829379876756 0.01000000828546632 0.1437811176768748 0.01000000000000001 0.09979540351020011 0.01000000000000001 0.01000004060792192 0.09271077718230636 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.3420852349358666 0.01000000000000001 0.5695589935515469 1.811936106004012 -0.1322823407948085 0.1569220878528954 1.249309862806559 -0.09860826332863036 0.9653381632774651 -0.009305978204525479 -0.3957168706854435 -1.359375170722483 0.01778383973392338 2.265499502000762 -0.918435326529973 0.1457791216646372 0.1208663656578314 -0.2013983438195694 5.193345771946677 -0.1306568571333695 9.645104855788809 +0.4178856253937646 -0.2803160930219124 0.02251123085342055 -0.08623133496987689 1.103952159596891 0.9176562906001862 0.2373721735576657 0.008677242073718894 0.1155472967347166 0.5107272435958041 0.636821447637415 1.551202843479013 1.551202843479013 0.7851300915499007 0.7851300915499007 -0.3259050862515975 -0.2822742685190942 -0.005707025444091851 0.3559851228756989 2.285069113588113 2.368397218840639 -0.001441836869132906 -0.1636471910084754 1.329174780875232 -0.05349810655551446 -1.573422919040889 2.507050053805322 -0.4389759414397814 -1.788595792986571 0.5068770889756078 1.638605274014725 3.068029507477147 -4.482939128578321 4.856751488738461 4.856751488738461 1.323741811967257 1.323741811967257 -1.42438575246978 4.981233069441965 0.648998417584342 5.742717691509849 -2.052756864531426 -2.893159440423402 -0.005399230545094724 -1.918548819229998 -0.06056305953179253 0.02260352644506547 0.01201622305384342 -0.05095247426183747 0.2212862472156809 -0.03192777604413077 0.1848297226774998 0.09813838176872869 0.04920227644451058 0.07174439356652851 0.002267606078397044 0.009999999999999787 0.03427007694148987 0.2029024637782495 0.1321957927261659 0.01000000950504898 0.04120887126056161 0.1283515445201551 0.04105455266057811 0.009999999999999787 0.0223161773013052 0.1026829766110851 0.1029165014345823 0.009999999999999787 0.04030053888043383 0.01000003675423455 0.03073614296619187 0.08329605256226635 0.08891347741398947 0.01468578425341693 0.03343517661765105 0.009999999999999787 0.01750637402282162 0.009999999999999787 0.02193634853156601 0.01029673996234282 0.01772828201089638 0.2978996712834014 0.2785401256452911 0.01016325302489207 0.005306500101342593 0.4581591490960637 0.4353216170731229 -0.3037840821222297 -0.01245703967602479 0.01256811518029299 -0.03728016523424899 0.3891303080107901 -0.22412741087311 0.3535741567902264 0.1388614607732287 0.08256886456869705 0.01000000000000001 0.2383864328690166 0.01000000071103979 0.1697130789472824 0.01000000000000001 0.1008614119888438 0.01000000000000001 0.01000004653633135 0.1082121113367694 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.3881681121291157 0.01000000000000001 0.5782555132654695 2.175982514751699 -0.0009071878811947382 -0.2704537155589 2.223078907765205 -0.05365058040544706 0.9690796248252403 -0.003064408836870934 -0.1349461773116584 2.260451656768041 0.2150359178168471 2.431200077433092 -0.5394871583468921 0.2397050155600552 0.1729931876246815 -0.1870248321223218 5.173753867578421 -0.07219746565139898 5.58090617397063 +0.424850385816994 -0.270028365809774 0.02152408864078836 -0.09650010230431327 1.121451321569881 0.9151580660741727 0.2236063707211504 0.01234531467712285 0.1261847480402163 0.5301193760663319 0.6035055160632736 1.581418984634627 1.581418984634627 0.7919501802373015 0.7919501802373015 -0.335967646369594 -0.2479373224180108 -0.001507734235224767 0.3928220419186044 2.272029215747498 2.348718039061747 -0.001146120338661127 -0.1757925895915164 1.614422195338915 -0.2248665696144276 -1.369503091095576 2.518112643354752 -0.2778455650640264 -2.152098205630347 0.5398929496619007 1.408703741747484 2.515273206467019 -5.06048575344086 3.827986072972452 3.827986072972456 0.6430680287622117 0.643068028762221 -1.459621495308875 4.861167747724506 0.5596026443421409 4.800509146580612 -1.688406369819454 -2.728785894505023 0.06603263619590383 -1.586606189354575 -0.09658822473640472 0.01554380605020267 0.01472956213242949 -0.0383522323805936 0.2458634440068601 -0.06080284985176343 0.2158958505746411 0.1179092723308131 0.06416902328679619 0.07730031209065524 0.002456035245292476 0.009999999999999787 0.03420621426987358 0.2271925924191893 0.1514731933374098 0.01000000528076184 0.04081772198226119 0.1299364066065394 0.04541062378922778 0.009999999999999787 0.02233825802832534 0.1034048063169282 0.1027075431472446 0.009999999999999787 0.04188234918553757 0.01000003760713142 0.03124673056499283 0.09913630717328381 0.1051232272821974 0.01227550657807708 0.03066903325565251 0.009999999999999787 0.01857310132366852 0.009999999999999787 0.0229992444637066 0.01014313665782662 0.01649560216100188 0.3359815596581956 0.3122655345798329 0.01007873397804548 0.005440210085021935 0.491522863108111 0.4651295849490311 -0.2190832320501863 -0.02032563315480473 0.04068215364430938 0.07170794065834452 0.3276313094926419 -0.1615728535702072 0.3601240616327535 0.145669423187747 0.08458959531551258 0.01000000000000001 0.2880480801780049 0.01000000188088124 0.09875233850249721 0.01000000000000001 0.1072451523961209 0.01000000000000001 0.01000003158663776 0.1265853032033357 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.4177225310028938 0.01000000368429654 0.5439564683199939 2.098467115907983 0.04962279017557561 0.123668729742251 3.288320125032419 -0.05798394395277187 0.1711921170690778 0.007517699114675609 0.08126553087577454 -0.8953469907455814 0.0007286252822922436 2.218421867913769 -0.2940070815174815 0.1083075493412401 0.1388241555330612 -0.1652532284549581 4.47294140047851 0.08649867406782158 2.938904946623318 +0.4318151462402234 -0.2579759788026328 0.01945002957528441 -0.1052263366966781 1.139029722811178 0.9137939529954107 0.2075657088824623 0.01610640683191589 0.1350676141105849 0.5459679958245367 0.5666596962986095 1.60463452357201 1.60463452357201 0.7942040849902048 0.7942040849902048 -0.3461595129058948 -0.2148083880816802 0.002126040108168414 0.4223683369648685 2.261597401962224 2.330792253212918 -0.0008603057205620601 -0.1859913904290833 1.83591433482002 -0.3656170559058043 -1.130469292979756 2.52980439523164 -0.1133045964556416 -2.441844107084056 0.5335885752989107 1.13475872984097 2.050474703829336 -5.496581515728356 2.846459161976671 2.846459161976671 0.0125168287438262 0.0125168287438262 -1.461517372348176 4.634340515690808 0.4866046434786857 3.649142735990623 -1.303819673056414 -2.389654991312843 -0.008242662844856863 -1.359563979212222 -0.1107637078634482 0.008337691196098174 0.02204006450590601 -0.008307165702353458 0.2548620805533561 -0.07313046810229196 0.2425248927424621 0.1314395991622712 0.07810696575116838 0.08114447043837636 0.002883696098911592 0.009999999999999787 0.03420352925697401 0.2606717409521409 0.1776620617359406 0.01000000391517908 0.04041076438328428 0.09886427847752133 0.04189833131206511 0.009999999999999787 0.02239460453949604 0.1066933618600796 0.1041365788067501 0.009999999999999787 0.04051679699504884 0.01000003008519146 0.031709438672884 0.1157630378947792 0.1197430374191466 0.01112835546393454 0.02879419690497631 0.009999999999999787 0.01959578536612439 0.009999999999999787 0.02396198495326818 0.01007087991222111 0.01545003853137006 0.3700006952351265 0.3403182632241868 0.01003899168533895 0.006175754231027675 0.5023449862006588 0.4757041699055211 -0.1343823819781429 -0.02819422663358456 0.06879619210832577 0.1806960465509382 0.2661323109744939 -0.09901829626730452 0.3666739664752806 0.1524773856022652 0.08661032606232832 0.01000000000000001 0.337709727486993 0.01000000305072279 0.0277915980577117 0.01000000000000001 0.1136288928033979 0.01000000000000001 0.01000001663694405 0.1449584950699019 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.4472769498766717 0.01000000758842801 0.5096574233745182 1.880551138664065 0.06779056257010911 -0.252885944409373 4.207775399522416 -0.05819009640068484 -1.290398986710441 0.006775350773232384 0.3354200226035808 1.414087298293807 0.2012868473371848 1.974633121959589 -0.2835395141665034 0.2270575576552145 0.1442325594150863 -0.1332925545140115 3.544787486779695 0.1006251802847226 0.05784960034069165 +0.4387799066634528 -0.2450559932261385 0.01655109149377054 -0.112420085207507 1.156690423691429 0.9135200355765045 0.1903176239500364 0.01981657236255385 0.1421372895171191 0.5595194599769759 0.5271837699533033 1.621299473270445 1.621299473270445 0.791539758005066 0.791539758005066 -0.3563103920424346 -0.1836981251640286 0.005200506556109374 0.4442305921642631 2.253875031912967 2.315870514124999 -0.0006072334029489923 -0.1949378086558 1.808550328607849 -0.4554232272212904 -0.9422032977144794 2.540664541261473 0.02720384407723309 -2.433420752700367 0.5327267582120783 0.9004999986663556 1.92643632889627 -5.772517502930894 1.957905421935468 1.957905421935488 -0.7823994113573236 -0.7823994113572876 -1.457387268364927 4.279275143363675 0.3911243565456801 2.652816254080037 -0.9133628572044885 -1.877304289104876 0.05864341502776593 -1.220478620673593 -0.09439005369821407 0.003278040547824546 0.032404235311426 0.02646852336731298 0.2553649380007164 -0.07392263723267245 0.2562989345369093 0.1389901013317667 0.08990061424068685 0.08251146943099474 0.003354541989034843 0.009999999999999787 0.03433716846352564 0.2874302328091556 0.2020382093418682 0.01000000336233153 0.03970016823249667 0.06400358327081079 0.03225804716564751 0.009999999999999787 0.02241157754732326 0.108315036132387 0.1066019893774799 0.009999999999999787 0.04146355039802518 0.01000002257825461 0.03259164126348679 0.1316612426473505 0.133121451034151 0.010544985220589 0.02732916568794241 0.009999999999999787 0.02065498543504773 0.02648116587737004 0.0373777347196862 0.01003417528226302 0.01479661851123248 0.3976765605978794 0.3620737154363161 0.01001880409900613 0.006987729724143854 0.4893225358724562 0.4636644240507533 0.08759182607337601 -0.01169292185246035 0.08950225561426384 0.1878159628268088 0.2496607463854208 -0.05727949786745307 0.2738040422766576 0.1460165271359846 0.08196365855362098 0.01000000000000001 0.3279433633199901 0.0100000026774264 0.0188957961860281 0.01000000000000001 0.1075013031666516 0.01000000000000001 0.01000001471064538 0.1585604274616437 0.01000000000000001 0.01000000000000001 0.06958098104533494 0.01000000000000001 0.453710251701934 0.01000000416108726 0.4188500823499994 1.470563050133629 0.06451525908263527 0.1723758928002873 2.561124115131864 -0.1541601938524639 -1.355698933514051 -0.002595055924146938 0.3361741061745209 -0.573260093618131 0.09284307316962104 1.877371781732395 -0.1557964506716942 0.1063340615893928 3.353208193044596 -0.0500024903044938 2.714151099367891 0.1272902505709095 -3.474856935721551 +0.4457446670866823 -0.2336980351023161 0.01326525682437607 -0.1184471191903889 1.174406361733678 0.9140692169588744 0.1747515600143097 0.02353963486033805 0.1476826248747418 0.573992939353845 0.4871818490867437 1.632169905794588 1.632169905794588 0.7832388485365609 0.7832388485365609 -0.3665156457376546 -0.1554787922952143 0.007502693598239851 0.4596550446058911 2.248879884924076 2.304892562087156 -0.0003536619906583383 -0.2031451570920355 1.387362666756636 -0.4767182465746131 -0.7954356501691704 2.54566629388607 0.1230563840381 -1.958835103306525 0.5372920672666728 0.6970293734004986 2.315240515864495 -5.64762605686716 1.182506112005804 1.182506112005804 -1.606083709049234 -1.606083709049234 -1.477131565914832 3.804181293210283 0.2648369561791153 1.80046385355921 -0.5206705352134549 -1.257157443533261 -0.008099342860056868 -1.147332762193626 -0.04080563988556385 0.002120614456324166 0.04464014078073797 0.05623891310867979 0.2527956678365455 -0.06701730954004326 0.2507719437646196 0.1404040570505178 0.09809602540903617 0.08105203124096461 0.003741935885562597 0.009999999999999787 0.03494861958047091 0.3001147988433566 0.2101202299526244 0.01000000290464031 0.03814783355762152 0.04006294599837812 0.02471635519840021 0.009999999999999787 0.02234879412687985 0.106428044736504 0.1083126651607271 0.009999999999999787 0.04045686503739176 0.01000001802886707 0.03357097669285025 0.1462383462413053 0.1460368549386573 0.01026994537031634 0.02636441238478104 0.009999999999999787 0.02148415313062735 0.06385554408404026 0.06572524318795381 0.01001691939720306 0.01481399573299402 0.4176496920987969 0.3782872942032443 0.01000931243409475 0.007875697202753162 0.4506082115898193 0.4278627862942539 0.3095660341248949 0.004808382928664034 0.110208319120202 0.1949358791026794 0.2331891817963481 -0.01554069946760156 0.1809341180780344 0.1395556686697039 0.07731699104491374 0.01000000000000001 0.318176999152987 0.01000000230413023 0.01000000000000001 0.01000000000000001 0.1013737135299054 0.01000000000000001 0.01000001278434659 0.1721623598533856 0.01000000000000001 0.01000000000000001 0.1291619675707917 0.01000000000000001 0.4601435535271963 0.01000000073374641 0.3280427413254808 0.8472816212112628 0.0438264953142809 -0.1156775176251029 -0.4712446976441581 -0.2999036580925717 -0.6877674382999053 -0.0161274982626541 0.118690446411803 0.8531379924421912 0.2291795048927322 1.841675293649168 -0.1398797517423624 0.161001481545743 4.432035095094802 0.05933352338387953 1.953383588151487 0.1224470322597872 -6.765613220436475 +0.4527094275099117 -0.2266396048068349 0.01008003487171427 -0.1235322089295128 1.192135256832954 0.9151146848252738 0.1641119595374709 0.02728922851914106 0.1518502878887853 0.5928952840016866 0.4496768329530947 1.638020982523129 1.638020982523129 0.769473666196177 0.769473666196177 -0.3769523911469177 -0.1308909195730799 0.008935830021712832 0.4693987012716323 2.24662373905451 2.298412892059454 -0.0001392148584300479 -0.2110042025976009 0.6204826902533753 -0.4318265823784655 -0.6651571163747323 2.545385751962232 0.1726185416819281 -1.071780931715714 0.5379604005088543 0.4987043409570768 3.129792861613153 -5.08653713496871 0.5025432070443143 0.5025432070443365 -2.319474877730261 -2.319474877730223 -1.52133172374081 3.253142571853113 0.1509703693305426 1.002767447716943 -0.1279959955923355 -0.6098212896160891 0.04942068003125977 -1.118583264083785 0.03440296997553371 0.002074171811113246 0.05270152529805783 0.07373281395776132 0.2456838567842157 -0.06160920133710768 0.2314212677106644 0.1399421438965138 0.1032648063411017 0.08435037605363194 0.004155334397996313 0.009999999999999787 0.03587518415069058 0.2899965918813008 0.1946125302585533 0.01000000228657827 0.0356272881826909 0.02513446932830776 0.02068767399363747 0.009999999999999787 0.02222616094992125 0.1040375408221861 0.108619750997887 0.009999999999999787 0.04094997988894411 0.01000001508704873 0.03468254194428599 0.1594966053619373 0.1586417646171485 0.01013020547790777 0.02588705911671951 0.009999999999999787 0.0220485864682618 0.09736364489747551 0.09394595034780107 0.0100081541731285 0.01560505042473537 0.4314071320506829 0.3893156230124757 0.01000448971142553 0.008836804203824045 0.3946209592911929 0.373654946410837 0.4383079291222383 -0.0009093039562133587 0.06962248229817136 0.1138340638864507 0.1947571482685876 -0.05730808068890186 0.1083781468716315 0.1391156851496765 0.097519785481882 0.01000000000000001 0.2313877619408194 0.01000000121435529 0.01000000000000001 0.01000000000000001 0.1008010232073284 0.01000000000000001 0.01000001198246314 0.182707466499086 0.01000000000000001 0.01000000000000001 0.141950641402704 0.01000000000000001 0.4575752650783247 0.01000000000000001 0.2413875967483772 0.6664206261768523 0.07936743193936761 0.2647958964574135 -3.835427323325296 -0.4110007880123216 -0.4343144286457878 -0.01848187035689043 -0.01150468875769439 -0.3663781339976848 0.1224878910630641 1.765889539944698 -0.01920261431610482 0.03174706452414285 3.573879365825717 0.1628856028631135 1.216902301958376 0.1497159538836648 -8.639112394101263 +0.4596741879331411 -0.225320405958799 0.00733541311891317 -0.1277174375423855 1.209862700833869 0.9164104491096445 0.1601655378533273 0.03101254250370467 0.1546146830330928 0.6178267816145611 0.4168271386135509 1.639237714576343 1.639237714576343 0.7513092540669133 0.7513092540669133 -0.3877272429614145 -0.1102104260140551 0.009665089620094935 0.4736961568589142 2.24708593155042 2.296310604648589 5.254230068540977e-05 -0.218853428489755 -0.2607145387049035 -0.3501955313338612 -0.5370316070786219 2.545249596605305 0.194931481744919 -0.03682246079165852 0.5297455982856492 0.2940670934465666 4.046571559220338 -4.310804253576841 -0.148291903627678 -0.148291903627678 -2.869366154234804 -2.869366154234804 -1.574222171967457 2.682138631100443 0.06271174805222435 0.2365332234941353 0.2599272744174992 -0.0001486303553179802 -0.01461499357551688 -1.144512962868311 0.119300338785759 0.001001315071581743 0.0519584585307733 0.07338230036597793 0.2329035334221259 -0.0647391819918095 0.2024411942036828 0.1394894513750518 0.1077889686953508 0.09545830570277047 0.004909924511307562 0.009999999999999787 0.03700798882562228 0.2508355855124322 0.1587354861807597 0.01000000141852375 0.03260241271109532 0.01757966085821971 0.01915122537253744 0.009999999999999787 0.02209979131456841 0.1024660990254023 0.108417113255677 0.009999999999999787 0.0401612653732375 0.01000001327932321 0.03572947164213591 0.1714073529914888 0.1704668609396709 0.01006447495674578 0.02579040110617736 0.009999999999999787 0.02235353606986346 0.119945901451425 0.11414339405568 0.0100040339116636 0.01701410131817793 0.4395894495428658 0.3952854083428519 0.01000222137957518 0.009907840688243574 0.3258091902226772 0.3097750030610262 0.5670498241195817 -0.006626990841090974 0.02903664547614071 0.03273224867022184 0.1563251147408272 -0.09907546191020217 0.03582217566522838 0.1386757016296493 0.1177225799188503 0.01000000000000001 0.1445985247286519 0.01000000012458024 0.01000000000000001 0.01000000000000001 0.1002283328847514 0.01000000000000001 0.01000001118057947 0.1932525731447864 0.01000000000000001 0.01000000396628298 0.1547393152346164 0.01000000000000001 0.455006976629453 0.01000000000000001 0.1547324521712736 0.6621730061193211 0.1418030458833746 -0.05645362704258676 -6.32050386704676 -0.4447270449749058 0.02790185460128584 -0.01720045933143523 -0.02768173254792932 0.4850479825581987 0.2106187649160149 1.617744157886995 -0.03055933456308502 0.08648815464944734 2.128075457409157 0.236797259146758 0.5007726807548479 0.1540143731781347 -9.543053614042565 +0.4666389483563705 -0.2300165120146374 0.005215450154825163 -0.1310031867194539 1.227599695908616 0.9178112523722919 0.1633451539910178 0.0346420392161666 0.1559310493987605 0.6487848119966899 0.389598635293658 1.635897595291246 1.635897595291246 0.7299688918090554 0.7299688918090554 -0.3988758876847567 -0.09346228374086873 0.009937095780199634 0.4726539873134832 2.250230592680228 2.298194596855816 0.0001855459568540496 -0.2271254490563499 -1.054596220142535 -0.2604658713427597 -0.4037485756721209 2.548764641192221 0.2080539060160982 0.9131649733668268 0.5093137763210056 0.08156122952999922 4.791422867953359 -3.522221373062178 -0.8173239826232239 -0.8173239826232235 -3.218696804531435 -3.218696804531453 -1.622924441868073 2.134415236274453 0.01898427909765354 -0.541679638399847 0.6364819685421845 0.5292823525355277 0.03497485969591319 -1.250620073459187 0.1880687942589483 -0.002369950873770676 0.04347428685625321 0.06342834325714142 0.2149530233921935 -0.07436130322486534 0.173368182357029 0.1398201827635952 0.1128052880914767 0.1060250172546424 0.005974319167046271 0.009999999999999787 0.038055473970251 0.1969759426901998 0.1150549765141107 0.0100000004105909 0.02949548634764199 0.01369683367614005 0.01933690466282467 0.009999999999999787 0.0220086838556286 0.1016423713903958 0.1082044292921918 0.009999999999999787 0.0402410194313978 0.01000001148833318 0.03677858396719813 0.1812982139622044 0.1808801348939877 0.01003108586169787 0.02589217915872366 0.01000000108062382 0.02256042350131349 0.1320792917030147 0.1241457543868774 0.01000194102495477 0.01875155834882936 0.4422393201439783 0.3962396944143229 0.01000106835447845 0.01104771152185213 0.2593012801136405 0.2449200114479648 0.4374664788978292 -0.02807947332261063 -0.01772041593352253 0.004584001521107939 0.1119161992015748 -0.1359121770928657 0.04658447262620513 0.1416402728238868 0.1200412317013608 0.01000000000000001 0.08256399481172938 0.01000000000000001 0.01000000000000001 0.01000000201249829 0.1009684412364119 0.01000000000000001 0.01000000868170925 0.1976670085156859 0.01000000000000001 0.01000000410690172 0.1418070327466556 0.01000000000000001 0.4409472490192435 0.01000000000000001 0.1314968711989686 0.7920835376139373 0.1617448669021043 0.2510085779647563 -6.11199065906832 -0.442137075247794 0.0502378265272528 -0.009591522389921314 -0.0273986277340757 -0.2563405272656879 0.1150066556597353 1.348187887582887 0.04085837953030817 -0.002984590316380291 0.7822959043660077 0.2501499313325242 -0.2320149642587001 0.1720000075233968 -8.869411789653215 +0.4736037087795999 -0.2395476504031331 0.003680879451803065 -0.1333030822033048 1.245374968254697 0.9193187206397551 0.1723740551325847 0.0380626517379028 0.1557176489728342 0.6838459124226337 0.3675680020930727 1.627762693645234 1.627762693645234 0.707031998961376 0.707031998961376 -0.4102739443197025 -0.08037932607440057 0.009979493404732143 0.4660689538780787 2.255859727117281 2.303517864596981 0.0002913138928266612 -0.2365489401669105 -1.649142004649775 -0.1820943403393875 -0.253933945907534 2.556241294526754 0.2255613952901174 1.642857714388098 0.4697624321661706 -0.1452206059513212 5.224808797687986 -2.818184320241524 -1.52516119117394 -1.52516119117394 -3.327825740157118 -3.327825740157118 -1.645843274906208 1.629644536832503 -0.003222619166271645 -1.355155091140538 0.9733684141738519 0.9874734341344786 -0.02243600062842477 -1.47516914456085 0.2209384107367596 -0.009013658405292446 0.02806326953834581 0.05018147987527666 0.1922139203867723 -0.08890887672551973 0.1515047916588754 0.1412895926952951 0.1190141321039029 0.1127205272181584 0.007133651362632776 0.009999999999999787 0.03902453471097367 0.1362365730407666 0.07514173187822992 0.009999999999999787 0.02651776830768826 0.01183488569511049 0.0201967453804559 0.0100000011550625 0.02195741637144932 0.1015307586286278 0.1081189563274947 0.009999999999999787 0.039457333689636 0.01000000932219036 0.03767081603344158 0.1888353601162196 0.1889073886643642 0.01001538933566248 0.0260958873069268 0.01000000265053735 0.02264757707307563 0.13307299260891 0.1255710383817785 0.01000095731702944 0.0203316829196738 0.4393031719954124 0.3919800729904512 0.01000052586889666 0.01228545915646873 0.2056613164407501 0.189171492413764 0.3078831336760767 -0.04953195580413039 -0.06447747734318582 -0.02356424562800619 0.06750728366232261 -0.1727488922755293 0.05734676958718177 0.1446048440181245 0.1223598834838716 0.01000000000000001 0.020529464894807 0.01000000000000001 0.01000000000000001 0.01000000780201382 0.1017085495880723 0.01000000000000001 0.01000000618283903 0.2020814438865851 0.01000000000000001 0.01000000424752046 0.1288747502586951 0.01000000000000001 0.4268875214090341 0.01000000000000001 0.1082612902266635 1.004618271777237 0.1690653015906944 -0.07897397209880386 -5.238696367025891 -0.407623593986302 0.2214939916956152 -0.00576001563376821 0.008848166881042523 0.2371026845165614 0.1655690433760564 0.9325748085686846 -0.001289097125493912 0.05210539012963428 -0.334917040800818 0.1916176836119659 -0.9964525441015433 0.1821206668789789 -6.928039736973821 +0.4805684692028293 -0.252318306675114 0.002617680874174688 -0.1344675047696682 1.263217430522589 0.9209938986921267 0.1854655305351334 0.04111566150914836 0.1538516593016408 0.7205923938837326 0.3499679443438843 1.614534651879096 1.614534651879096 0.6842307621628576 0.6842307621628576 -0.4216893419945582 -0.07063392968953863 0.009940714524066863 0.4535499168338157 2.263607442378942 2.311818465496925 0.0003532571108708638 -0.2480200651538906 -1.975966679996157 -0.1278436910940259 -0.07887189099505498 2.568498296544746 0.2583505288055399 2.068517542529763 0.4040923608494444 -0.3904028659597452 5.267448575409545 -2.270753093753647 -2.28005860706262 -2.280058607062613 -3.201067394274592 -3.201067394274578 -1.627110188837639 1.176688776311028 -0.008683148173378008 -2.241866535110554 1.240376621542037 1.384058453230603 0.02428143103999592 -1.829902312514421 0.2254697495009097 -0.01744288032041297 0.008624364015099584 0.03505567684068467 0.1695726246605442 -0.1029815943253882 0.1333725368298229 0.1358245240027109 0.1222366429783239 0.1124729147528636 0.008277154596642688 0.009999999999999787 0.03975260722994323 0.08425655468715298 0.04480964824729483 0.009999999999999787 0.02386050430480058 0.01088719633015689 0.02138065297695446 0.01000000256505285 0.02193316158988168 0.1019481465076129 0.1082993826628078 0.009999999999999787 0.03912334943125106 0.01000000742638241 0.03843949023558579 0.1932541649383128 0.1934327711798285 0.01000741691311102 0.02626381667100741 0.01000000219188735 0.02274778005878053 0.1284907004203855 0.1211333573878086 0.01000045787948212 0.02133638662087822 0.4303057599402655 0.382153464183598 0.01000024994831916 0.01364988440141701 0.1658268728984025 0.1482039843054594 0.1878258137610053 -0.06153547891938133 -0.09390615958539189 -0.0430892490616222 0.06608079378112441 -0.1613491501103044 0.04511266015299698 0.1145007008010368 0.1033357148202081 0.01000000000000001 0.01611645649638482 0.01000000000000001 0.01000000000000001 0.01000000059066708 0.1031094804248842 0.01000000000000001 0.01000000522430655 0.1978562025819193 0.01000000000000001 0.01000000000000001 0.1168869629391429 0.01000000000000001 0.3990265570401165 0.01000000000000001 0.1050712554829998 -0.1579114565919444 0.1560889488597666 0.1902128908914781 -3.457909951112464 -0.3537964054684066 0.1406995543270327 -0.00209865487656234 0.04613555743575957 -0.2123266767292392 0.07736580797551172 0.3567748372187224 0.03259334877773952 -0.002619242989225075 -0.8754023381300301 0.1000014481734388 -1.824906519700897 0.2063659073277362 -4.866556982926801 +0.4875332296260587 -0.2664853213693368 0.001835584132108714 -0.1343798654306938 1.281168236715497 0.9229573687415216 0.2005192907624003 0.04365180417343639 0.1502825044798213 0.7563851391213054 0.3354512707379795 1.595909848306922 1.595909848306922 0.6627032581348931 0.6627032581348931 -0.4328678924177289 -0.0638793223868741 0.009847815200459298 0.4348121876912376 2.27298312570184 2.322629034937586 0.0004074756818934766 -0.2621921643881726 -2.050120168247272 -0.1013731615844069 0.1056080424039201 2.587353326652369 0.3083473001912127 2.206347254710415 0.3213389184923194 -0.6343013631141181 4.950933986310071 -1.93275999668986 -3.074894844054552 -3.074894844054552 -2.962060748078342 -2.962060748078342 -1.577830238618656 0.7708098029883885 -0.01876384085759675 -3.140914175565011 1.44085558165887 1.708235782933599 -0.02465420959802023 -2.250780675519669 0.2074523229759677 -0.0265189996312003 -0.01262254821521358 0.01913368145361982 0.1507709053668878 -0.112444161028153 0.1163077619593693 0.1194504777145999 0.1157186779111146 0.1037310477840805 0.00926312560376541 0.009999999999999787 0.04031132390217751 0.05125099633857388 0.02716347865279367 0.009999999999999787 0.02161239415850424 0.01043954522301815 0.02246617491935554 0.009999999999999787 0.02191573472622466 0.1028131517989044 0.1088057941292186 0.009999999999999787 0.03818077977106826 0.01000000602729845 0.03905775393609145 0.1941430768475616 0.1937355839015509 0.01000367039582795 0.02631423280785983 0.009999999999999787 0.02289959865035396 0.1206046582446358 0.1142686582200625 0.01000022353218899 0.02176795514733643 0.4148977127970488 0.3665662549141242 0.01000011994919214 0.01511376821880672 0.1397857151766742 0.1209596370169361 0.06776849384593353 -0.07353900203463226 -0.1233348418275979 -0.06261425249523822 0.06465430389992632 -0.1499494079450797 0.03287855071881196 0.08439655758394926 0.08431154615654446 0.01000000000000001 0.01170344809796253 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.104510411261696 0.01000000000000001 0.0100000042657743 0.1936309612772533 0.01000000000000001 0.01000000000000001 0.1048991756195908 0.01000000000000001 0.3711655926711989 0.01000000000000001 0.1018812207393363 -1.792455956130928 0.1238283609743181 -0.1276061824007533 -1.595810390742169 -0.2901310079572139 0.1932415757841438 -0.003799311311215804 0.1024580289053327 0.06234135331718778 0.122377176747721 -0.2799779677257517 -0.03503418528251152 0.06692732327537264 -1.03185936943502 0.02703623646280391 -2.650666074084973 0.2106811819300233 -2.987298064315094 +0.4944979900492881 -0.2803853407748749 0.001140323998892523 -0.1330369323883041 1.299277779805632 0.9253248830723297 0.2156408982855735 0.0455901371576517 0.1450885007755125 0.7888804661167184 0.3224476473903781 1.571647323591336 1.571647323591336 0.6428752452649631 0.6428752452649631 -0.4436439695750058 -0.05981017432259694 0.009653715592983669 0.4098889784346156 2.283559031667743 2.335415930002691 0.0004413167373713556 -0.2794090028924563 -1.930645180468213 -0.09961370358215804 0.2748648302438292 2.613673868166369 0.3716507100379092 2.124024292186003 0.2366812363215578 -0.8494683822972009 4.365987372107586 -1.820111150532791 -3.889943758014135 -3.889943758014111 -2.741942292127518 -2.741942292127522 -1.520616590384716 0.4022897776133005 -0.03991591492659952 -3.998312026347062 1.592541910535222 1.946769174054432 0.02019163929398182 -2.678340344976782 0.1796196350298382 -0.0350457905574979 -0.03423487068054598 0.003888146659279279 0.1362661682461082 -0.1142160749561114 0.09770075970584946 0.1016983597238328 0.1041235345719791 0.08799538140888519 0.01004146881539913 0.009999999999999787 0.04061849978493726 0.04352062854845329 0.02296615618530851 0.009999999999999787 0.01978819048287228 0.01021206813352604 0.02351961964120708 0.009999999999999787 0.02187881929200275 0.1038673845545328 0.1096884231600166 0.009999999999999787 0.03741438696607213 0.01000000526539768 0.03966645794646562 0.1914501115262452 0.1896551322701354 0.01000176925447693 0.02616409639383654 0.009999999999999787 0.02323088035702625 0.1125478378577611 0.1073664952197562 0.01000010502016435 0.02177436662683707 0.3938033164588961 0.3454202329357741 0.01000005371562107 0.0166164255664758 0.1206349046954545 0.1037201985621317 0.03957909498267109 -0.0739693505720283 -0.1408625298031329 -0.06789133636057626 0.07582696254268884 -0.09619454616284212 -0.005291406985265978 0.07489283318905238 0.05568390372207566 0.01000000000000001 0.05478948676587603 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.105738463495294 0.01000000000000001 0.01000000479913488 0.1820010833217157 0.01000000154802194 0.01000000000000001 0.09996021155480583 0.01000000000000001 0.3355087470320069 0.01000000000000001 0.08966450845913232 -1.529172267261204 0.09383487187052612 0.1266852825486169 0.3489594864316515 -0.2380470780411568 0.1285557233991236 -0.007261079746755092 0.150696325251444 -0.2119459175775579 0.06905656001228971 -0.8759184767399851 -0.0183565694158573 0.04068770461187544 -0.9368253181661944 -0.01892232980575645 -3.397610313434717 0.218207663191109 -2.067183220644137 +0.5014627504725175 -0.2932283599455978 0.0004294027761564401 -0.130623057406436 1.31758510572068 0.9281362741903743 0.229939502061804 0.04696828183117008 0.1385576301573361 0.8170001935417974 0.3098367910348916 1.541758030001298 1.541758030001298 0.6243673380395838 0.6243673380395838 -0.4541049239801143 -0.05821149573757811 0.009250823180068757 0.3793644537889502 2.295116621183192 2.34951148421279 0.0004912098543692345 -0.2992931020496181 -1.746592072353594 -0.1058703986950693 0.4131396370020113 2.644148625527087 0.4358109199759141 1.970013570010641 0.1604752917046213 -1.018195950328633 3.694437192607589 -1.81997306125287 -4.690677306779136 -4.690677306779136 -2.582979325522654 -2.582979325522654 -1.487339616393057 0.06139000126453587 -0.07872076478596624 -4.749387417620884 1.722755744738518 2.084030229684187 -0.02004470100290989 -3.016711013619944 0.1517223875093139 -0.0421072412109238 -0.05510793272384706 -0.009553241090801912 0.1264086725074547 -0.1059384366532146 0.07555314231527444 0.08730385581482114 0.09452992470954591 0.06574168714265216 0.01048877209986765 0.009999999999999787 0.04083445796153473 0.06064212341291242 0.03144557129675629 0.009999999999999787 0.01823605946539253 0.010105019156323 0.02452559784830655 0.009999999999999787 0.02180818715354471 0.1049989491820034 0.110900724462347 0.009999999999999787 0.0362101260176444 0.01000000518994115 0.04025144291375415 0.1850961502213688 0.1817551821243315 0.01000087845956577 0.02591537261704158 0.009999999999999787 0.02364025782910328 0.105808973388581 0.1014049480250856 0.01000004999519399 0.02159175307377836 0.3673867823207666 0.3195736671131839 0.01000002243316889 0.01811688313033288 0.1041055496834908 0.09071601664177731 0.01138969611940877 -0.0743996991094244 -0.1583902177786677 -0.0731684202259143 0.08699962118545135 -0.04243968438060464 -0.04346136468934403 0.06538910879415571 0.02705626128760674 0.01000000000000001 0.09787552543378986 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.106966515728892 0.01000000000000001 0.01000000533249545 0.1703712053661782 0.01000000685885849 0.01000000000000001 0.09502124749002105 0.01000000000000001 0.2998519013928151 0.01000000000000001 0.07744779617892861 -1.217695776261553 0.02876696419385271 -0.1538002510199045 2.044440474657595 -0.2120026470056875 0.1796110955210799 -0.01348139718332428 0.1971266899254894 -0.06339682478612672 0.1155660183872486 -1.376784830412534 -0.06334452383811566 0.08935329670259375 -0.7617522162380375 -0.0272441755142315 -4.000473837339421 0.2100494922008759 -1.771102164742678 +0.5084275108957469 -0.30488177001493 -0.0003081668661337744 -0.1273705082816012 1.336108033521004 0.931366213540989 0.2432795967389274 0.04785628826677346 0.1310341245358946 0.8405823659502278 0.2971921284290668 1.506428324351542 1.506428324351542 0.6067994262282044 0.6067994262282044 -0.46448660755315 -0.05890630412659981 0.008596248371375736 0.3440088779060773 2.307569345690879 2.36417891494877 0.0005508525564454914 -0.321112998594522 -1.612300104795705 -0.1037975894076961 0.5167105483709959 2.674441525355701 0.4893095689678137 1.875211445063958 0.09538209786117058 -1.135556384208932 3.095547382240702 -1.797782561581304 -5.446309784289179 -5.446309784289179 -2.461329891541766 -2.461329891541767 -1.49941468569871 -0.2555330127764277 -0.1073979187718352 -5.389467886766483 1.852186979996063 2.112896572443999 0.02392336745304835 -3.227232267669252 0.1291648820878444 -0.04731042946550001 -0.07402024137413399 -0.01789565661734649 0.1174880862332532 -0.09094806077242268 0.05380828036531904 0.0742609051373968 0.08555568223917387 0.04512298163817086 0.01069548524715636 0.009999999999999787 0.04095789000069505 0.08164454501254292 0.04439106443567109 0.009999999999999787 0.01676982120192605 0.01005063940241646 0.02571125506815708 0.009999999999999787 0.02171822547877955 0.1062925137583632 0.1123479633449027 0.009999999999999787 0.03505174873334793 0.01000000581432481 0.04086285362706477 0.1763163294560628 0.1707540490099051 0.01000043405486206 0.02567653850224705 0.009999999999999787 0.02406572344733293 0.09992391309082826 0.09642621766164927 0.0100000260983224 0.02145028809134208 0.3374288137955235 0.2901222492410067 0.01000000644369159 0.01960182063508764 0.08659754738627123 0.07814245625125338 0.04120149534861839 -0.06767297686314222 -0.1613006339290197 -0.0393970653739964 0.06755730074843447 -0.004477514352742351 -0.04629645591149129 0.053605634444855 0.01896415757245262 0.01000000000000001 0.1118104119338111 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1087046628552196 0.01000000000000001 0.01000000728628125 0.1574567964758351 0.01000003152083051 0.01000000000000001 0.09061184044436243 0.01000000996774797 0.2648636609293127 0.01000000000000001 0.05500448094452148 -1.351217851353005 0.03324621848104877 0.1045186792811587 1.711305961948648 -0.2105213441974368 0.1709884397951004 -0.01225936303550428 0.2171288046095064 -0.2282168445122864 0.07217442528294282 -1.767971305701039 -0.01619682145733274 0.0436432146933242 -0.6786438735214273 -0.01219315472256796 -4.431790860657641 0.2164065319870597 -1.884798351484244 +0.5153922713189764 -0.3158610461911406 -0.0009867439512638576 -0.123483356009439 1.354832495879986 0.9349188497654382 0.2562620622754879 0.04830885385099171 0.1228331582747511 0.8603722273824674 0.2849795894234415 1.466008993919104 1.466008993919104 0.5900890288560201 0.5900890288560201 -0.4750683741241639 -0.06169603373612764 0.007780576784472526 0.3044846557186713 2.320903039510996 2.378734577502509 0.0006399080568897375 -0.3439424531193778 -1.553007842001013 -0.08893010108788912 0.5953722218322501 2.702021271938662 0.5284765263204028 1.867316816997436 0.03543413452990851 -1.212735488957567 2.605437501111422 -1.695884485622829 -6.15221351861573 -6.15221351861573 -2.336735553425182 -2.336735553425181 -1.544795432766924 -0.5401873950014573 -0.1249813016730945 -5.946455529433496 1.975734816205975 2.051934888151894 -0.01159852437029141 -3.306615595714773 0.1160854717949107 -0.05035447454723041 -0.09003643634945968 -0.01861608203827902 0.1066632165628216 -0.07180011870540115 0.03548581645969762 0.06155623398683829 0.07582134142983588 0.03012900038791422 0.01098883732323408 0.009999999999999787 0.04111015624572678 0.09872183059314299 0.05581701909424908 0.009999999999999787 0.01528299432308211 0.01002507189026902 0.02704843954142344 0.009999999999999787 0.02163871034365217 0.1077847961957774 0.1139066496853887 0.009999999999999787 0.03360264517630362 0.01000000714324578 0.0414262896458677 0.1657206617621605 0.1573277752685081 0.0100002382391704 0.02553712228659188 0.009999999999999787 0.02439890210435536 0.09466658511749415 0.09180265814038746 0.01000002198036221 0.0214384250868429 0.3049837795911521 0.2581892085273774 0.009999999999999787 0.0211319004889754 0.06626738309050184 0.06383097782370051 0.07101329457782801 -0.06094625461686004 -0.1642110500793718 -0.005625710522078498 0.04811498031141737 0.03348465567511982 -0.04913154713363843 0.04182216009555428 0.01087205385729839 0.01000000000000001 0.1257452984338325 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1104428099815472 0.01000000000000001 0.01000000924006716 0.144542387585492 0.01000005618280253 0.01000000000000001 0.0862024333987037 0.01000002339195949 0.2298754204658103 0.01000000000000001 0.03256116571011447 -1.4359655493274 0.05364619143528462 -0.1455202229766705 1.608089936129546 -0.2179148720148956 0.2231236484901741 -0.01048156976447363 0.2291299322583849 -0.1468817560653098 0.1017896092643909 -2.07318800285513 -0.03479568853358961 0.06285218107403141 -0.65976138805069 0.009972317247538807 -4.713096437586125 0.2230125652688093 -2.270162557745368 +0.5223570317422057 -0.3267304412875056 -0.001510417176349144 -0.1191092825950251 1.373733367708398 0.9386867834182659 0.2695356815339127 0.0483446035404711 0.1142044198247443 0.8771880744072407 0.2738806000175389 1.420855404102328 1.420855404102328 0.5743732493439202 0.5743732493439202 -0.4860435433654051 -0.06633569668341499 0.006879235182793764 0.261206084200527 2.33505561448904 2.392611869587592 0.0007332753027928796 -0.3671056286956569 -1.570168473458937 -0.06091415478414408 0.6580808823736888 2.724976147297049 0.5515047045126114 1.945744284328083 -0.02496701537565427 -1.260808162500408 2.230564247828019 -1.486434744864753 -6.805055047673244 -6.805055047673251 -2.170133705654576 -2.170133705654578 -1.604895120217358 -0.783498252719611 -0.1359583035021461 -6.463533405516172 2.083147301491101 1.928548176760419 0.02639676563200632 -3.31018960130197 0.1061507307531029 -0.05267940082695688 -0.1032659013501642 -0.01381499428697186 0.09262859685278713 -0.0527108620816783 0.02241338604407295 0.04985986009319276 0.06609777295730268 0.02031779508810239 0.01137720487457861 0.009999999999999787 0.04122839317246063 0.1136756334197835 0.06634139404510808 0.009999999999999787 0.01373018149159533 0.01001208487083183 0.02861352262718952 0.009999999999999787 0.02157092664494709 0.1095076638998638 0.1155509161512276 0.009999999999999787 0.0321425795535295 0.01000001014102647 0.04194972065002966 0.1527289611182296 0.1419138323360718 0.0100001367288276 0.02552349689238032 0.009999999999999787 0.02462099707885557 0.08164564810200181 0.08551966694175617 0.01099897766605951 0.02223132593971311 0.2700375817949308 0.224648331747348 0.009999999999999787 0.02277136225229137 0.0472338116784643 0.04858638438803453 0.05165702347450207 -0.06539058740014575 -0.1625058033256999 0.02079625395212992 0.0108220956677233 0.03441066949222538 -0.02546323794058331 0.03318667564050992 0.01043602822535683 0.01000000000000001 0.1378607825598014 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1127181821548718 0.01000000000000001 0.0100000160250211 0.1257708394515407 0.01000003051417275 0.01000000000000001 0.04899412029864658 0.01354882955563397 0.1912238475338263 0.01000000000000001 0.02128058036565983 -1.354672838424654 0.06814536852239501 0.09881767096158342 1.435233640527255 -0.2257709049013174 0.2292460588909244 -0.009810014383458012 0.2472550894729673 -0.2477354819718229 0.05866086162371456 -2.340636234019591 0.01709761809131047 0.01287986717719371 -1.307297370438737 0.2114631733170197 -4.894697787373262 0.2452161391204196 -2.070126093922088 +0.5293217921654352 -0.3377595098817787 -0.001827814725779753 -0.1143528971523375 1.392782094301179 0.9425729325012417 0.2833849849111614 0.04796387848640649 0.1053303806340931 0.8915427176474284 0.2643420412196731 1.371343654431327 1.371343654431327 0.5599446949155249 0.5599446949155249 -0.4973967974103273 -0.07248944401559454 0.005857345498282207 0.2146998189036298 2.349848266433066 2.405535383025449 0.0008402657805368641 -0.3895680317998993 -1.598859669832435 -0.02969599593059247 0.7051569107650459 2.744479233270988 0.5624232590046265 2.032630218705452 -0.08416071676097436 -1.283176028399513 1.898678435462834 -1.247791367347924 -7.403702324937466 -7.403702324937466 -1.967096109857306 -1.967096109857306 -1.653366232250232 -0.974971735850569 -0.1595978786620322 -6.873319795431849 2.159539262326975 1.778047894410823 -0.007686478295972243 -3.1053818333291 0.09451082752238005 -0.05538790937093152 -0.1137923971274217 -0.005100837082070875 0.07438456384138803 -0.03690889262598951 0.01599050401239266 0.03946301778804395 0.05697341617686202 0.01523951891404263 0.01208109752309472 0.009999999999999787 0.04136314717442291 0.1270658971493295 0.07610367541101892 0.009999999999999787 0.0121690348438408 0.01000598015951049 0.03028268822390956 0.009999999999999787 0.0214905442940565 0.1114750424728417 0.1174095438366676 0.009999999999999787 0.0304952420633664 0.01000001514587989 0.0423846815031137 0.1370303469805778 0.1248971490627193 0.01000007541199732 0.02558325984987331 0.009999999999999787 0.02474484373061436 0.05669928511882372 0.07132441098397369 0.01334119733244865 0.02429689263857426 0.2325483726540045 0.1903399508360919 0.009999999999999787 0.02451199825618255 0.03150127311243978 0.03551534573428716 0.03230075237117613 -0.06983492018343151 -0.160800556572028 0.04721821842633844 -0.02647078897597083 0.03533668330933104 -0.001794928747528068 0.02455119118546556 0.01000000259341527 0.01000000000000001 0.1499762666857705 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1149935543281965 0.01000000000000001 0.01000002280997514 0.1069992913175895 0.01000000484554309 0.01000000000000001 0.01178580719858924 0.01709763571930867 0.1525722746018422 0.01000000000000001 0.01000000000000001 -1.263895972021406 0.144252350999731 -0.1407771811025913 1.389247208265997 -0.2203080099705163 0.2530119733208784 -0.01409939763091429 0.2906858813516514 -0.2006587292726919 0.07638452834049174 -2.533430780369242 -0.01372149581295462 0.03463920135668518 -2.931846650943954 0.3754309313456834 -4.933492857464121 0.2520657514309607 -1.64600475481375 +0.5362865525886646 -0.348822741364792 -0.001946004197520779 -0.1093308357346556 1.411959687270993 0.9465063543288825 0.2976234220718443 0.04718611415208462 0.09639056674071167 0.9035089414590836 0.2563069581726602 1.317862768772325 1.317862768772325 0.5470463872104991 0.5470463872104991 -0.5089782059181873 -0.07976417313908035 0.004589825989161689 0.1658870372531593 2.365032070748922 2.417397765581313 0.0009342227314097507 -0.409539686698619 -1.573667726679775 -0.005131223369970961 0.7346727658131533 2.762117629948576 0.5654024536631468 2.049658704725834 -0.1379750640156461 -1.280874640169065 1.537602796219411 -1.06359119147968 -7.944309367850043 -7.944309367850043 -1.729267537807844 -1.729267537807845 -1.667501759385343 -1.105290382145758 -0.2056610534928209 -7.083320425305962 2.193823870708755 1.631483288790708 0.02424178256805387 -2.493739031904459 0.080787413563562 -0.0583949986391068 -0.1225084756673764 0.005222359930728171 0.05497344294958451 -0.02295764755405649 0.01338353747739385 0.03041978435601589 0.04866909849987255 0.0125465404732239 0.01314653462899207 0.009999999999999787 0.04138749579549517 0.1332347517973669 0.08351274413948495 0.009999999999999787 0.0106818898064005 0.01000287947243272 0.03198112476958714 0.009999999999999787 0.02136591285011935 0.1140495206379599 0.1197566983546179 0.009999999999999787 0.02881286247887127 0.01000002421182966 0.04275522824742595 0.1211844444355057 0.1070260411389805 0.01000003819542883 0.02561074910548999 0.01837271223037362 0.03252251371870729 0.03465510428625329 0.04963259241510443 0.01626695652577936 0.02695736768254609 0.1963761700303412 0.1565378738426122 0.009999999999999787 0.02627790481207803 0.02069886924672826 0.02626414685976508 0.005464524601928256 -0.07413211500269801 -0.1632899327848758 0.05668347615339164 -0.03945832001405969 0.04514023028378722 0.004693057551792168 0.0184091277298184 0.01000000000000001 0.01000000000000001 0.1338068967506147 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.119299009949673 0.01000000000000001 0.01000004089820095 0.09453678164025403 0.01000000245365185 0.03998787369746382 0.01089295976041504 0.02066942606200228 0.128890581086348 0.01000000000000001 0.01000000000000001 -1.12086040435844 0.1646994565248532 0.07500940713617396 0.7046154443439983 -0.2077389911885912 0.2378904080798586 -0.02125472552137889 0.3859419479037548 -0.2670154730485038 0.03743926044120123 -2.560102538814548 0.01298327627323092 2.026589516798644 -3.010581012430887 0.3886793133885758 -4.717630170983726 0.2487816910033649 -1.005458573272961 +0.543251313011894 -0.3596188078589684 -0.001911658799006233 -0.1041513657211732 1.431250953327957 0.9504251022313874 0.3118463285836044 0.04605876430417455 0.08753166896639186 0.9129616436547754 0.2494706061959069 1.260816325290778 1.260816325290778 0.5359614830324766 0.5359614830324766 -0.5205567638156026 -0.08776374747997195 0.002974781692591133 0.1168743750778689 2.380312227024076 2.428304768976911 0.001032721848694074 -0.4224110728496546 -1.522140594773567 0.01410590420244207 0.750358349505273 2.777121587132835 0.5582094959333062 2.02817043890494 -0.1845476625702442 -1.259940162658397 1.176895785887781 -0.9035638570161677 -8.427587600487788 -8.427587600487788 -1.446359310071683 -1.446359310071684 -1.652547651201484 -1.183118225711314 -0.2593930665765436 -6.930717469625612 2.18720768261727 1.503701297483619 -0.006382150183128044 -1.066469274594738 0.06469076785118055 -0.06163557856026536 -0.1300976101436429 0.01539028220340066 0.03672489994791528 -0.009757076515336749 0.01242262732460997 0.02272654882441039 0.04135912486415183 0.0112629776168105 0.01441708425694754 0.009999999999999787 0.04139448421189851 0.1295756021218217 0.08544885948607073 0.009999999999999787 0.009261420759265171 0.01000142228965295 0.03364067639502455 0.009999999999999787 0.02120053593217719 0.117388108586181 0.1228220136302411 0.009999999999999787 0.02699088525266946 0.01000003805927863 0.04300951632328465 0.1065147285193184 0.08977010052346657 0.01000001943476558 0.02564387060038786 0.03766703200753696 0.0505755130987664 0.0226519936912819 0.03338056426993186 0.01951414747223712 0.02971279864348242 0.1639154827514986 0.1253984115816631 0.009999999999999787 0.02789034667394708 0.01533825495079411 0.0215809713747368 -0.02137170316731973 -0.07842930982196461 -0.1657793089977236 0.06614873388044473 -0.05244585105214855 0.05494377725824329 0.01118104385111252 0.01226706427417135 0.01000000000000001 0.01000000000000001 0.117637526815459 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1236044655711498 0.01000000000000001 0.01000005898642653 0.08207427196291872 0.01000000006176083 0.06997574886657543 0.01000011232224085 0.02424121640469601 0.1052088875708539 0.01000000000000001 0.01000000000000001 -0.9783602233380089 0.2031524450727169 -0.1457623359879233 -0.1823656107272033 -0.2011613383699919 0.2418457829933273 -0.02579986442817881 0.4969128624505543 -0.2407455759679098 0.04299969917336889 -2.356779389416967 -0.01210414246971499 2.985301899933814 -1.369756904006065 0.4026984643546304 -4.168896641447294 0.2079968431829498 -0.334251553837049 +0.5502160734351234 -0.3700754106646675 -0.001752963557319465 -0.09889933794717365 1.450635666887953 0.9542516098592118 0.3259168772574257 0.04463330807969257 0.07886688095222594 0.9200131706575565 0.2437747636615564 1.20058904230482 1.20058904230482 0.5269686713179351 0.5269686713179351 -0.5319591330100326 -0.09615786484584277 0.001064738656295638 0.06991076397279983 2.395408683371649 2.438410749863666 0.00112538265456541 -0.4233012274769719 -1.482088857080099 0.03121861986722818 0.7572283202567922 2.789075498261944 0.5389417484312404 2.013402635687828 -0.2241622278361914 -1.227164100748437 0.8515697601962149 -0.7322202124359136 -8.861756097251003 -8.861756097251003 -1.142055844504055 -1.142055844504055 -1.620180599570578 -1.222409051139422 -0.2829801648787087 -6.56136074415745 2.144732689904861 1.402640883556561 0.02354925996496071 0.7798839515872933 0.05148582853396544 -0.06510063567161284 -0.1363550492908274 0.02370050097970466 0.022106715684163 0.001356684792472418 0.01235158693601068 0.01709564936537955 0.03603674965233949 0.01061013310905956 0.01582556974175819 0.009999999999999787 0.0413295309850259 0.1180405884702953 0.08151609381040315 0.009999999999999787 0.007871584099280682 0.01000068253435016 0.03527405964447361 0.009999999999999787 0.0210401944728833 0.1210628213217739 0.1264239328208276 0.009999999999999787 0.02512432097585915 0.01000005524765379 0.04317476950211185 0.09575781331942101 0.07460384032750023 0.0100000090168173 0.02570411648207882 0.06031192448132883 0.07174287379856992 0.01621698869294441 0.02604695922232336 0.02200100133614225 0.03207260950956092 0.1388979886073627 0.0989391311756056 0.009999999999999787 0.0293296569046908 0.01259491089064824 0.01957283375483954 0.00367688621698814 -0.08309653439201775 -0.1638124300186671 0.05699159914092478 -0.03656402277827436 0.04887409304625234 0.01282736899518211 0.01113359108651457 0.01000000000000001 0.01000000000000001 0.09047299527166774 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1276616821065907 0.01000000000000001 0.01000008152548515 0.08283502013519217 0.01000000000000001 0.09655966111912029 0.01000009184386386 0.02454463387153039 0.1032861474223149 0.01000000000000001 0.01000000000000001 -0.5558982478877483 0.2017294943387791 0.05889169089377979 -0.9587937942309308 -0.1972154188397618 0.2280865265709915 -0.01964950659501288 0.5284710185683216 -0.284966047761712 0.01158837328215024 -1.989659299020969 0.02006058463589094 3.085824011943035 -0.7634977155724765 0.2742647551039707 -3.423716873881606 0.2070974209350328 -0.2288419526563507 +0.5571808338583528 -0.3802846610435582 -0.001480230446654573 -0.09361168920046037 1.470097042719913 0.9579090631699949 0.3399070442686893 0.04294498383799095 0.07045284241519356 0.9248730474802898 0.2392688113232069 1.1374531674824 1.1374531674824 0.5199690563414787 0.5199690563414787 -0.543103162868569 -0.1047226927269005 -0.0008818235013299258 0.02539402349005071 2.410143354072545 2.447902914855166 0.001229238146380762 -0.4119790408110764 -1.451110893311476 0.04685300878258269 0.7605872649456944 2.799135918511061 0.5096638728911889 2.005088047645913 -0.2600328196317241 -1.187934964520102 0.5475417099788569 -0.5618750913723507 -9.262818580363225 -9.262818580363225 -0.8739931073642015 -0.8739931073642015 -1.578362949877469 -1.232139442469661 -0.2698798942759471 -6.228074313605552 2.083317347650337 1.327437978159472 -0.003167485136267523 2.440425593621112 0.04520423078966918 -0.06878326789896594 -0.1411240044856039 0.02884937859711734 0.01300859746274163 0.009360488227998065 0.012591365146287 0.01376909822040417 0.03353374412345778 0.01030222731720087 0.0172329489303511 0.009999999999999787 0.04126456369344123 0.09923286472220072 0.0719285803193781 0.009999999999999787 0.006524453766926008 0.01000033538914291 0.03683021132637698 0.009999999999999787 0.02093510559659739 0.1248884674328687 0.1300587910838145 0.009999999999999787 0.02316479083514267 0.01000007527170732 0.04327030978840307 0.09015930310588161 0.06217633230675323 0.0100000033946146 0.02579315083246492 0.08402096679027649 0.09345799515768327 0.01309130089909516 0.02236461242259669 0.02334677266416207 0.03352370507804414 0.1232231802476291 0.07778332939815691 0.009999999999999787 0.03079993779846513 0.01128700729307885 0.01858224036729084 0.02872547560129624 -0.08776375896207089 -0.1618455510396105 0.0478344644014046 -0.02068219450440018 0.04280440883426129 0.0144736941392517 0.01000011789885769 0.01000000000000001 0.01000000000000001 0.06330846372787635 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1317188986420316 0.01000000000000001 0.01000010406454377 0.08359576830746573 0.01000000000000001 0.1231435733716653 0.01000007136548708 0.02484805133836487 0.1013634072737758 0.01000000000000001 0.01000000000000001 -0.1687502690023301 0.2028347611601989 -0.1457663748270393 -1.806182503886198 -0.1888977930241256 0.219667972742935 -0.009933537465950418 0.5063716436005449 -0.2674416453915238 0.02298095099044591 -1.570319551948692 -0.003837248296292564 3.142596803411728 -0.3212619016942653 0.1417518580773638 -2.645950177704406 0.2168903760977491 -0.04205364018843265 +0.5641455942815823 -0.3902788673096822 -0.001101976056701037 -0.0883024824844143 1.489625044765161 0.9613349224750696 0.3538311544615809 0.04101040556534352 0.06232528335462639 0.9276286040848705 0.2359058968700873 1.071594956584406 1.071594956584406 0.5146391872144558 0.5146391872144558 -0.5539354905902629 -0.1132683285711069 -0.00263404812973933 -0.0174363964169606 2.424425704597439 2.456953201950396 0.001330181025791966 -0.3907426267759204 -1.420468225138769 0.06171639543798335 0.7634006093538499 2.808291714777512 0.4727595530137445 1.994733923629944 -0.294959464298203 -1.145019655881637 0.2485343218068619 -0.4043251903656802 -9.642844197796176 -9.642844197796171 -0.6617792531081521 -0.6617792531081523 -1.532460860837481 -1.221002472818516 -0.2475795500713924 -6.079172621376377 2.018082234119388 1.274275013968735 0.02396971481046517 3.615376435919664 0.04137232966602111 -0.0718590083059647 -0.1460964596840317 0.03190289792444112 0.006382044290212718 0.01184897443566957 0.01215492716657796 0.01182786336802 0.03289266103225286 0.01014578451505699 0.01860551631574747 0.009999999999999787 0.04111537253263986 0.08469376105991078 0.06239109884016036 0.009999999999999787 0.005243241469066184 0.01000015975932378 0.03829191170393198 0.009999999999999787 0.02088589812789277 0.1268074336258884 0.1330045251956982 0.009999999999999787 0.02118422142195087 0.01000010517455729 0.04334642911996234 0.08103072348824991 0.05205556069647965 0.009999999999999787 0.02588473643976474 0.1038487080179702 0.1125076887380643 0.01149759869777833 0.02070284893726715 0.02516419965862982 0.03486936759442916 0.1044195800112924 0.06105656842002105 0.009999999999999787 0.03244576031139168 0.01062176464453035 0.01820175668468327 0.01950085845361305 -0.08399791591447364 -0.1751011996833288 0.04382068423419228 -0.02655084069641839 0.004642029958721983 0.006124809223513661 0.01000008744244629 0.01000000000000001 0.01000000000000001 0.07037209653225274 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.127223556898967 0.01000000000000001 0.01000015726884507 0.05882984282262815 0.01000000000000001 0.1315660754972493 0.01000003526134186 0.02871120147579342 0.06263611598056718 0.01000000000000001 0.01000000000000001 -0.002728856711647645 0.1902040056350884 0.04096518592032221 -0.9161925690476435 -0.1784619241918599 0.2014317566199557 -0.008485513547200446 0.3375047767878029 -0.2928479532349225 0.00334649861301348 -1.372983103056556 0.02246161271764873 2.333198163910805 -0.167055029802527 0.2502736730583069 -2.219148280382712 0.2504263583920193 -0.05353085118848844 +0.5711103547048116 -0.4000939989252359 -0.0006212484102348093 -0.08298629049776851 1.509211972635784 0.9644756275899473 0.3677118948001636 0.03884387633323527 0.05451656347252154 0.9284017427584641 0.2336297060009604 1.00321898801726 1.00321898801726 0.5106776918368734 0.5106776918368734 -0.5644525826102993 -0.1217192201971855 -0.0045295516089916 -0.05939831138430796 2.438255401496832 2.46569242220571 0.001449118362778279 -0.3622098149086308 -1.39968783214295 0.07627922024651301 0.762593154644569 2.816057053423845 0.4277790203290994 1.992633964018272 -0.3266398370624728 -1.096377133642959 -0.02172925142876814 -0.2498116411321734 -9.985817654598527 -9.985817654598527 -0.4810510199949634 -0.4810510199949634 -1.487839893087517 -1.204932700995859 -0.3110238366639337 -5.978674510349244 1.953333371749905 1.238112051344136 0.00199980174120773 4.535658563232817 0.03656442407156391 -0.07369651995955806 -0.1525680544254122 0.03367733712328924 -0.0001073385589149822 0.006980235473001972 0.01028644945936907 0.01090615947592521 0.03367144276234058 0.01007219133343007 0.01986697174599161 0.009999999999999787 0.04097212340221823 0.07999194307564395 0.05939500241517059 0.009999999999999787 0.004046269025840488 0.01000007810000758 0.03965497512780347 0.009999999999999787 0.02075716777837577 0.125895365974555 0.1347319286506181 0.009999999999999787 0.01920327117437948 0.01000014742641264 0.04337917634248845 0.06429555635673267 0.04253562630233265 0.009999999999999787 0.0259991163784159 0.1180038401046062 0.1260347534522928 0.0107422542510931 0.01988264896719061 0.02802675007442534 0.03708797516835949 0.0757805181191511 0.04601021068536282 0.009999999999999787 0.03421446512147197 0.01030799098815693 0.01802706982506486 0.01027624130592997 -0.08023207286687639 -0.188356848327047 0.03980690406697984 -0.03241948688843654 -0.03352034891681743 -0.00222407569222427 0.01000005698603468 0.01000000000000001 0.01000000142099411 0.07743572933662912 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1227282151559024 0.01000000000000001 0.01000021047314636 0.03406391733779046 0.01000000000000001 0.1399885776228331 0.01000000000000001 0.03257435161322209 0.02390882468735855 0.01000000000000001 0.01000000000000001 0.2389779900285011 0.1709278617425707 -0.1440600619120939 0.07224106194703353 -0.1647074836638392 0.1913433936655524 -0.0327693210359644 0.1565159177015786 -0.2675510057566988 0.0105262199079768 -1.397783188972745 0.002708369920534595 1.556719176858838 -0.07959859552388639 0.3924266851830332 -2.163405727836202 0.2521772459079766 0.0170429576803429 +0.5780751151280411 -0.4098280903006506 -4.250698039243161e-05 -0.07770024157945965 1.528845708731502 0.9672710651324601 0.3816363611412155 0.03647936737405111 0.0470752362059117 0.9274728766769238 0.2324511322070166 0.932665606236295 0.932665606236295 0.5079966624829195 0.5079966624829195 -0.5746774155930043 -0.130078718906685 -0.007297626264334944 -0.1003879590026928 2.451649059528166 2.474233009318451 0.001587313353605069 -0.3275336828594702 -1.381908189452964 0.0882413540766338 0.7542641644257666 2.822243216294012 0.3748172260978744 1.989186031453005 -0.3508839321842054 -1.039369932504831 -0.2570191767374008 -0.1064247497850093 -10.26699007939966 -10.26699007939967 -0.2829901876601499 -0.2829901876601499 -1.446467666906805 -1.196587076916736 -0.5028825763269253 -5.775011329812563 1.890612511927209 1.21763234586101 0.02865675539194967 5.379573959523696 0.0294842700785618 -0.07347602363290306 -0.1630174638631829 0.0332864304666054 -0.007846579996980019 -0.005730762977531612 0.009304291811881971 0.01043754734241542 0.03520591027311148 0.01003480812391189 0.0209827066703161 0.01000000013615931 0.04077558310652929 0.07033660371338302 0.05503365555845541 0.009999999999999787 0.00300753089506145 0.01000949565370668 0.04094447790217703 0.009999999999999787 0.02039456489028479 0.1213974492965022 0.1351220879111099 0.009999999999999787 0.01738963607033917 0.01400626982949493 0.04662470069681612 0.04668844638792535 0.03277983685308072 0.009999999999999787 0.02611807669643973 0.1338147410420332 0.1385807672531185 0.0103582903473991 0.01950497383628003 0.0350351763120238 0.04151177958420948 0.04946652000278373 0.03142113876735353 0.009999999999999787 0.03594761895487464 0.01014856549765408 0.01807330951144426 -0.01484317985672706 -0.06517197689609472 -0.231272162062701 0.02370201915137693 -0.05293309849160599 -0.09161435519864358 0.01149994496573126 0.01000004927136333 0.01000000000000001 0.01000000109391752 0.04371790720569313 0.01000000000000001 0.010033565075384 0.01000000000000001 0.1093698182063129 0.01000000000000001 0.02427871581364671 0.02203195509736622 0.01000000000000001 0.1669147750856349 0.01000000000000001 0.05010955451025001 0.01695440800994352 0.01000000000000001 0.01000000000000001 0.2262944626449226 0.1505285696592734 0.0275152330066599 -1.320601180319288 -0.133155867339239 0.1775766803849353 -0.07209464426885025 -0.06966971262240464 -0.2505111668060704 0.8544462904309257 -1.386473581259346 0.02111100902924141 2.080550576615505 -0.0274187763214207 0.8636340211658193 -1.965450162375145 0.2444978732996217 0.004806655904940314 +0.5850398755512705 -0.4191532521255708 0.0005846384213099576 -0.07249485212186624 1.548528345113689 0.9696946829303248 0.3951872823622065 0.03397668565425294 0.04005401411625931 0.9246542210630535 0.2318993531726337 0.8603063552690964 0.8603063552690964 0.5068171636052732 0.5068171636052732 -0.5845753505262139 -0.1384011298013048 -0.01179949266540303 -0.1396060735513998 2.464560420088241 2.482698451674215 0.001722544422814831 -0.2878650173686546 -1.282259657850004 0.0901789006149385 0.7394309908894714 2.830099255450847 0.3210072946528806 1.885366779707327 -0.3663175150341536 -0.9757397674083768 -0.5644010498575522 -0.06982131224277532 -10.50439089372027 -10.50439089372027 -0.04987165844837671 -0.04987165844837671 -1.393967877509162 -1.194282567291783 -0.8088993242069333 -5.469961227466293 1.814836871669467 1.216533489436712 0.001148682265709411 5.969314033979957 0.01913926673511668 -0.07056977201433456 -0.1793427376344958 0.03005151849420562 -0.01789779972938765 -0.02665543675437521 0.01098374210196074 0.01021674022948904 0.03716676848233913 0.01001723351232009 0.02197855696021778 0.01000000052158878 0.04051814497279072 0.04876590269523184 0.04105598987682679 0.009999999999999787 0.002197330361866534 0.01003171054521257 0.04210940196345314 0.009999999999999787 0.01974263068961912 0.112912579806252 0.1334105519178488 0.009999999999999787 0.01575195323779166 0.02332987428274791 0.05434775895441391 0.03137018396672264 0.02346250872271316 0.009999999999999787 0.02614913388227214 0.1540798091540925 0.1554971957303737 0.01017745103990775 0.01952071836264935 0.04722311151367853 0.04891908939922907 0.03176263037115445 0.01947573515333723 0.009999999999999787 0.03760603443785282 0.01007356744771126 0.0182134201411408 -0.03996260101938404 -0.05011188092531293 -0.274187475798355 0.007597134235774128 -0.07344671009477538 -0.1497083614804698 0.02522396562368689 0.0100000415566921 0.01000000000000001 0.01000000076684082 0.01000008507475725 0.01000000000000001 0.01006713146411109 0.01000000000000001 0.09601142125672346 0.01000000000000001 0.03855722115414706 0.01000000000000001 0.01000000000000001 0.1938409725484365 0.01000000000000001 0.06764475740727793 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.3614203060537047 0.136501800505197 -0.1615474798267131 -2.689180219284117 -0.09908064693306813 0.1555693812148881 -0.1158536756976801 -0.4470067094486582 -0.2170245667095806 1.296294467830168 -1.271874674587665 -0.02253388590811325 2.811727076845775 0.03337575919511446 1.249169837414945 -1.404250166595921 0.2307153876713102 0.04399892963629726 +0.5920046359744999 -0.4272034747959212 0.001170336285611473 -0.06741049554817424 1.568283528071195 0.9717651466805046 0.4073224822159021 0.0313982597513851 0.03349241856893359 0.9190681546638135 0.2309040036867271 0.7863814750933211 0.7863814750933211 0.5074507577015512 0.5074507577015512 -0.5940115848901717 -0.1467018224332426 -0.0187965527001337 -0.1762844975690379 2.476849587097234 2.491223583211556 0.001792714595373113 -0.2452136333919057 -1.012048081521986 0.07647045057847812 0.7213211595480193 2.842929203647363 0.2738179038467106 1.577782484539622 -0.3734292747195447 -0.9094387927547212 -1.051210090888201 -0.229649129017369 -10.69640169593585 -10.69640169593584 0.2610566260153515 0.2610566260153515 -1.311259304674046 -1.189457670509725 -1.199048138954963 -4.966588991657562 1.707588609906857 1.229015191252479 0.01279740428273124 6.203420642218219 0.007617321680562394 -0.06801571252633343 -0.1956818039807735 0.0275587925614289 -0.0292795917806159 -0.04826159689823051 0.009574378689709562 0.01010455747015593 0.03943741152171398 0.01000830656975271 0.02287206669386066 0.01000000011894775 0.04003421100448668 0.02973911217599934 0.02673285417515459 0.009999999999999787 0.001557748024064054 0.02071445357587187 0.05336122965459866 0.009999999999999787 0.01878852891938987 0.103681835412901 0.129549148090438 0.009999999999999787 0.01417846235438791 0.03121825964021419 0.06108717978005673 0.02612446378716982 0.01645752224441832 0.009999999999999787 0.02597053609134203 0.1807750100413368 0.1795090826611712 0.01008557967538692 0.02003310953447368 0.05121368537575099 0.05434958718222394 0.02083284901096771 0.01215677966557704 0.02722308445524124 0.05481234891846665 0.01003547019355011 0.01851415528837341 -0.04915354945315353 -0.06235736349444232 -0.2658002862923068 0.02437840677629199 -0.08802797626270525 -0.1432111964734226 -0.017582087021975 0.01000003706461894 0.01000000000000001 0.01000000000000001 0.01000005179550978 0.01000000290085989 0.04836634916446259 0.01000000000000001 0.08876076826283041 0.01000000000000001 0.03938268528228162 0.02991810986267507 0.01000000000000001 0.240435852602415 0.01000000000000001 0.0449633927595966 0.01000000000000001 0.07231685325240766 0.01000000000000001 0.3123821270936135 0.1191606210549702 -0.02755710637048983 -1.485019710044404 -0.08535885374104339 2.800608787702829 -0.1541310159365069 -0.6399198269724463 -0.2287872365342601 0.664329415307781 -0.6903658760726594 -0.03115393308904119 4.075468772760804 0.1116119030188272 0.1943917224402497 -0.7140163691016757 4.227183723302667 0.05010384387266916 +0.5989693963977293 -0.4330083137768517 0.001628389311155054 -0.06243704275670181 1.588130823074447 0.9735126101318987 0.4168642468274166 0.0287843800698413 0.02737271563018773 0.9098509447195831 0.2285103690319041 0.711693243226494 0.711693243226494 0.510860886342861 0.510860886342861 -0.6027781453184682 -0.1549712940258345 -0.02848316462141076 -0.2074507262190766 2.488255342366771 2.499782839580754 0.00181438774569731 -0.202499514690075 -0.6374754720213371 0.0535244197692224 0.7075838703894646 2.8565501425314 0.2282545568258882 1.140636140125244 -0.3764984190469951 -0.8488459241951181 -1.607119915807797 -0.4713527602843715 -10.72359489922049 -10.72359489922049 0.7474377610724146 0.7474377610724146 -1.201664368131111 -1.18531713633021 -1.581230811063603 -3.887079683018827 1.561156433070325 1.226332773198553 -0.01277771843774334 5.987328932180422 -0.003482746324319663 -0.06814015847984534 -0.2075457716655347 0.02855438468603877 -0.04124044288637529 -0.06478145713781602 0.0006728066033874214 0.01005179447272297 0.04182127471511476 0.01000411036494731 0.02362562341154506 0.009999999999999787 0.039435887282671 0.01992661510776728 0.01951831474570653 0.0100000023358362 0.000997497910852907 0.04518052443131237 0.07729137512293249 0.009999999999999787 0.01765130918064184 0.09523610158760043 0.1248020248110397 0.009999999999999787 0.01264907538379667 0.0354143025162923 0.06395441734281127 0.03368050647849996 0.01453291811550983 0.009999999999999787 0.02568172009171965 0.2147028823416401 0.2121543133976882 0.01004237632143745 0.02104546266839247 0.0419132445004311 0.0500130026548411 0.01540569574548512 0.009299213338672008 0.06622769080898649 0.08975981890569429 0.01001755959088602 0.01901920863520523 -0.05834449788692303 -0.07460284606357181 -0.2574130967862588 0.04115967931680975 -0.1026092424306351 -0.1367140314663755 -0.06038813966763684 0.01000003257254589 0.01000000000000001 0.01000000000000001 0.01000001851626253 0.01000000622929131 0.08666556686481397 0.01000000000000001 0.08151011526893737 0.01000000000000001 0.04020814941041606 0.04983622686840816 0.01000000000000001 0.2870307326563933 0.01000000000000001 0.02228202811191515 0.01000000182334371 0.1346337115045876 0.01000000000000001 0.3939325956757086 0.09631290456092871 -0.1943956315541777 -0.6478763292550245 -0.07629961218844472 3.796274016009213 -0.1684378156858705 -0.7013498304015707 -0.2043596909254261 0.1843586574255429 0.1870074809394981 -0.05418413717391089 5.290877336271641 0.1769447929399301 -1.555544673231749 -0.1231180415072881 5.325264066704028 0.1026709261408393 +0.6059341568209586 -0.4362276538515433 0.001921522635163431 -0.05751777952934356 1.60805656879138 0.9749208279757955 0.4233527325519733 0.02614758940732864 0.02162696643222706 0.8970591735609146 0.2246743321554994 0.6377102997556774 0.6377102997556774 0.5183420392090317 0.5183420392090317 -0.6107426047856208 -0.1632670707863362 -0.04054513408645244 -0.2278734348651903 2.498494871834314 2.508206384491318 0.001809487636386553 -0.1639895390638877 -0.2915016180245789 0.03115016084409294 0.7070131328946048 2.864405714706689 0.1755419904298123 0.7264615564994488 -0.3813717192195556 -0.8033321678695451 -2.044056809766236 -0.6154542908640219 -10.46344116499947 -10.46344116499946 1.407459455147031 1.407459455147031 -1.081361728831964 -1.197408345112167 -1.850589248525555 -1.978702161967522 1.374616141418616 1.185799901241577 0.005661532150962767 5.05470239097159 -0.009034251917997338 -0.07050949913755389 -0.2148675580562442 0.03383648624444824 -0.04991023672127604 -0.07417669587815601 -0.00974632551914878 0.01002499044156746 0.04423508008890131 0.01000197905528699 0.02420349932632471 0.009999999999999787 0.03879352289182947 0.01485836817516351 0.01591789135043786 0.01000000766776798 0.0005319781032699922 0.08363076991453511 0.1087071795285119 0.009999999999999787 0.01652952343650549 0.08790312389510468 0.119729231875763 0.009999999999999787 0.01135374365781372 0.03373367795294469 0.06166210056392396 0.03636775336064835 0.01712192844547111 0.009999999999999787 0.02545515794581688 0.251271923802074 0.2495108394942971 0.01002043065265301 0.0223336624705901 0.03055988500215889 0.03772850191083243 0.01262796374702502 0.009777651769109053 0.0967478352293929 0.1189119503538865 0.01000846189987792 0.01977573685250089 -0.01154750070261851 -0.0875412901625085 -0.2393541486992139 0.07350317788736693 -0.07672047503388418 -0.09811945783735598 -0.05391346650029311 0.01000002780355791 0.01000000000000001 0.01000000000000001 0.01000001183457355 0.01000001799581918 0.1605874854135233 0.01000000000000001 0.07702236964255282 0.01000000000000001 0.0251040698546392 0.02991812630539403 0.01000000000000001 0.3291548447542494 0.01000000000000001 0.01614101210313579 0.01000000000000001 0.1295963130098813 0.01000000000000001 0.3179274133146993 0.07024821515976863 -0.03911475114510438 -0.3817199067320264 -0.05374614704740627 5.096567584250466 -0.1490882676634648 -0.7527653170191737 -0.1620108202228913 -0.8023583270415747 0.4732878242647608 -0.0187463813622307 5.410187824694868 0.1906412211663651 -1.830452565338654 0.2276287521941638 2.968089905458259 0.1147273911474658 +0.6128989172441881 -0.4371316370852907 0.002069239165925385 -0.05256108304611606 1.628017813197488 0.9759496469941431 0.4270373416126598 0.02346244951679832 0.0161516310675287 0.8816861550985653 0.2201428316888601 0.5667490347444586 0.5667490347444586 0.5305581963943924 0.5305581963943924 -0.6177846178049378 -0.1716577357541142 -0.05381671733879756 -0.2350297951416476 2.507338873970967 2.516206136755531 0.001813725547782141 -0.1323196293631121 0.02740219792335408 0.01176654601339955 0.7183332009023482 2.866743943041982 0.1193064229364484 0.3354751601344015 -0.3903823791766592 -0.7711939112525816 -2.348350668014528 -0.6710620246412429 -9.855889974461643 -9.855889974461643 2.107141191327313 2.107141191327313 -0.9367747260537058 -1.212565225714312 -1.928583401434008 -0.07751623401699392 1.160416925586315 1.104636513577098 -0.01015364520276307 4.023136193371543 -0.005111427225186205 -0.07479169635338323 -0.2175957945150242 0.04401631361587288 -0.05232542524760397 -0.07488646495469187 -0.01710792050814813 0.01001238734845344 0.04651049858640111 0.01000097751543683 0.02461276031914661 0.009999999999999787 0.03820780736082874 0.01241335695808754 0.01426105069678441 0.0100000164098164 0.00029944395121273 0.1359082673140648 0.1464939243701071 0.009999999999999787 0.01563873782738101 0.08182124810569036 0.1143524091138062 0.009999999999999787 0.01047030351758726 0.02516836585414861 0.05333877813309762 0.02751322866937622 0.01996714662670085 0.009999999999999787 0.02531095338711076 0.2887038451997141 0.2871502433444921 0.0100101140866462 0.0236685013703104 0.02166780041762406 0.02648835934728311 0.01130342999462375 0.01201200318873852 0.1097882202661626 0.1300178103642931 0.01000418545471549 0.02061944774531188 0.03524949648168596 -0.1004797342614453 -0.221295200612169 0.1058466764579239 -0.05083170763713324 -0.05952488420833646 -0.04743879333294937 0.01000002303457015 0.01000000000000001 0.01000000000000001 0.01000000515288457 0.01000002976234715 0.2345094039622329 0.01000000000000001 0.07253462401616806 0.01000000000000001 0.01000000000000001 0.01000002574238013 0.01000000000000001 0.3712789568521053 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1245589145151746 0.01000000000000001 0.3541934322659372 0.04789345818081277 -0.178128349880361 -0.08975727746018543 -0.009395471525329296 5.625752720074911 -0.1021038369117544 -0.7886550093006526 -0.08608093882918615 -1.547504403338785 0.2605801052846337 -0.03053433956644051 5.372108487499082 0.1903376721415321 -1.255646868546762 0.3811097804033903 0.1430983233316922 0.1277062956901695 +0.6198636776674176 -0.4357339216910732 0.002092385510851891 -0.04750050143080342 1.647983507231387 0.9765957437094857 0.4278819333706627 0.02070068671886682 0.01087172163841199 0.864482753336913 0.2153102263378726 0.5013259476990726 0.5013259476990726 0.5475171547099036 0.5475171547099036 -0.623663545324147 -0.1800936437801495 -0.06682236305171418 -0.2323454707642361 2.514627843625653 2.52349221064396 0.001819574359753773 -0.1070124968823407 0.3685858904905226 -0.004571634195583485 0.735775510167217 2.865402329600693 0.06610603287786132 -0.08813283973087582 -0.4027030434873149 -0.7464415045760013 -2.569275751826601 -0.6988019327012069 -8.877237992212038 -8.87723799221204 2.771976945785921 2.771976945785922 -0.742503574783232 -1.20811783646861 -1.786813236338746 0.7698670239615319 0.9300040127407803 0.9757865182720868 0.00561194511541574 3.263879736755104 0.006866054191480675 -0.07960454778871995 -0.2176424779764936 0.05921593831537786 -0.05269473447261541 -0.07269908982331685 -0.02676342404683885 0.01000598464442604 0.048570998723243 0.01000046966422596 0.02489206862465609 0.009999999999999787 0.03760540981333982 0.01116793813841044 0.01359496606602484 0.01000003126378779 0.000368954276865896 0.1958240731847569 0.1844577876314606 0.009999999999999787 0.01513756493491769 0.07680931842884364 0.108667147895749 0.009999999999999787 0.01005530347181427 0.01851591895574112 0.04585002380382708 0.01915781294574881 0.02006562061906259 0.009999999999999787 0.02518524464395799 0.3317446360690517 0.32746766440897 0.01000487358708435 0.02502225928648416 0.01572512139784044 0.01955967309682771 0.0106297145430907 0.01389227988453001 0.119631798521747 0.1296423781014115 0.0100020132299723 0.02149098970517649 0.08572471235879398 -0.1023765015179433 -0.2146151748954448 0.1492986651064881 -0.05770407031893443 -0.06582036543114039 -0.0924108279884252 0.01000002300200875 0.01000000000000001 0.01000000000000001 0.01000000453714855 0.0100000579370263 0.3204327672385913 0.01000000000000001 0.06965902756186648 0.01000000000000001 0.01371977701910621 0.01176965359192872 0.01000000000000001 0.4403472609047181 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1386711833614276 0.01000000000000001 0.2534883428619607 0.03084664386026473 -0.0371468291513557 -0.0830484697848142 0.02787343571761171 5.22665463257968 -0.04461704651055982 -0.8300301216072633 -0.03078219037427798 -0.6796065636075301 -0.17770906448754 -0.007651179875053349 6.179020738054342 0.1986207310286267 -0.7490801740020183 0.1767530656609697 -0.1386466527467675 0.1270861368508114 +0.6268284380906469 -0.4320723494849394 0.002013194786675054 -0.04229937159619679 1.667914769545222 0.9768687921642667 0.4258766863612413 0.01785274034119189 0.005733762123157238 0.8462108022703045 0.2106577284545841 0.4438422147241812 0.4438422147241812 0.5692984515881734 0.5692984515881734 -0.6280031688500911 -0.1884615614154699 -0.07843728163124641 -0.2253990991317556 2.520256067672938 2.529633375841049 0.001805238300327972 -0.08657923814487578 0.6774911239919867 -0.01762055788778705 0.7586930153817537 2.856856533291086 0.01218191673898605 -0.4828842935319697 -0.4151308484714136 -0.7304315862796908 -2.655195193903709 -0.6193423798289381 -7.576025747140534 -7.576025747140534 3.491923515387646 3.491923515387646 -0.494748324653604 -1.193041111010065 -1.529222318304223 1.146372886624956 0.6835173607833651 0.7758646329224743 -0.01594993112540299 2.623555031318142 0.0258110749310001 -0.08388962969504732 -0.2164717232856233 0.07952883680946243 -0.05424100336325033 -0.07204699036603657 -0.0428107146019765 0.01000297503932401 0.05026418514988817 0.01000023240264669 0.02502201795855008 0.009999999999999787 0.03710127992565582 0.01057874190478181 0.01336146257028448 0.01000005317727881 0.0006670554399357265 0.2621567009025365 0.2186121004364971 0.009999999999999787 0.01497818371357207 0.07277346981549293 0.1029840238315511 0.009999999999999787 0.01007367514385216 0.01720576986016109 0.04280461300948168 0.01597489218911363 0.01825220242903303 0.009999999999999787 0.02517530510113319 0.382598238142815 0.372853052615949 0.0100024099679743 0.02643816550439126 0.01284566793078668 0.01584388932052638 0.01031193031101996 0.01472374406231891 0.131224672710907 0.1296502676129654 0.01000099214364125 0.02245266539906776 0.136199928235902 -0.1042732687744414 -0.2079351491787206 0.1927506537550522 -0.06457643300073573 -0.07211584665394449 -0.137382862643901 0.01000002296944713 0.01000000078747609 0.01000000000000001 0.01000000392141243 0.01000008611170555 0.4063561305149496 0.01000000000000001 0.06678343110756491 0.01000000000000001 0.01743956373934985 0.01353928144147731 0.01000000000000001 0.5094155649573308 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1527834522076803 0.01000000000000001 0.2487156590551033 0.005003548815037544 -0.1499098172126416 0.0344626838179164 0.05624665355272551 4.531802541134339 -0.003954970246250666 -0.788041304299323 0.0383660461298482 -0.2715542584484554 -0.2884386882375862 0.002709933973678366 6.827425493721122 0.208184173087 -0.3330300722526991 0.07993286856083195 0.2531729708757471 0.1535890909486822 +0.6337931985138763 -0.4265872663071684 0.001855722954648531 -0.03691452421393393 1.6877496084902 0.9767474292886664 0.421467199897736 0.0149223303185515 0.0006670643792707764 0.8280198172084363 0.2072363079488055 0.3964074960946289 0.3964074960946289 0.5965741740198895 0.5965741740198895 -0.6304460591889507 -0.1967346145586242 -0.08801995987085487 -0.2156632683012254 2.524106527569375 2.53408059709504 0.001751724812693478 -0.06995391503708026 0.8864330932615507 -0.02748192300497498 0.7884966590055331 2.837884506765173 -0.04753638473966859 -0.7712457779681658 -0.4258207377985053 -0.7259031130973104 -2.549148490714568 -0.3437267470166572 -6.020759768095783 -6.020759768095783 4.345884749453585 4.345884749453588 -0.2059893328867375 -1.18540476197878 -1.200561191576138 1.627870132125443 0.4185806373993546 0.4922070572984638 -0.003923586210166086 2.155867041024201 0.04291200953864394 -0.08893531492192164 -0.2155501606464432 0.09154425220141604 -0.05679010634505977 -0.07481381178907887 -0.05695923155773119 0.010001446793519 0.05152371419758994 0.01153162638043392 0.02552636177137035 0.009999999999999787 0.03671727434586058 0.01027929354513057 0.01326588660014716 0.01000007460299912 0.001108207411730788 0.323325508290436 0.2457512235638539 0.009999999999999787 0.01506931796153044 0.07633876352269242 0.1009173284865468 0.009999999999999787 0.01053429943521822 0.01629068870254935 0.04007115773563008 0.01423122273470145 0.01646976127462807 0.009999999999999787 0.02538591277102231 0.4265614942319402 0.4136572152748448 0.01000115839009386 0.02792006773107136 0.01137805829547034 0.01381638932604456 0.0101504665457437 0.01547297184756724 0.137552925946677 0.1301337034830148 0.01000047345951982 0.02359306350638324 0.1058296916058873 -0.1189726283275141 -0.2145957689899913 0.1021789982088119 -0.07192976722114985 -0.1017429416181894 -0.1059626725232938 0.01000002461920857 0.01544386911080342 0.01000000000000001 0.0100000008568959 0.01000010162230336 0.4482581188587192 0.01000000868494355 0.09203521933361525 0.01000000000000001 0.01371982787583881 0.01176963813812026 0.01000000000000001 0.5144762605853482 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.139986877397886 0.01000000000000001 0.1272105206733348 0.1481286376358057 -0.002403571020922218 -0.03772373847354834 0.07083775012628023 3.23181165762637 0.03319223354670006 0.18627218025079 0.09547448156675671 -0.494554475819831 -0.2372888465830169 0.04990231264956467 4.783077963848963 0.2164824056547975 -0.2496213354557061 0.1249823925945412 -0.1386092483891598 0.1724980295203072 +0.6407579589371057 -0.4198803789586787 0.001632016147680826 -0.0313037696532974 1.707430799194695 0.9761996138054241 0.4153021170989146 0.01192884893576807 -0.004396974002176979 0.8109723116297389 0.2061403701125837 0.3603180832633317 0.3603180832633317 0.6299085676913672 0.6299085676913672 -0.630861880009594 -0.205012080556136 -0.09485438712488348 -0.2030230955506749 2.526036552841959 2.536364330333845 0.001687810000886358 -0.05647527629474869 1.028344303850605 -0.03664058755600941 0.8235645011609594 2.812734917859766 -0.1102781771420287 -0.9870211851508606 -0.4332424147883867 -0.7296679239929693 -2.326833724536453 0.04844302218305785 -4.318108716578118 -4.318108716578118 5.231731023944681 5.231731023944681 0.08734478265315992 -1.194308305889385 -0.7400348032427515 1.980383460850746 0.1320455205023192 0.1545976018345523 -0.01893677447248265 1.719953421431343 0.05142134111044738 -0.09572817712996207 -0.2160008645863924 0.08499287500693997 -0.06020870646866117 -0.08244195855401326 -0.06286046428047953 0.01000072933496199 0.05223452211592683 0.01511761702440761 0.02720106163696068 0.009999999999999787 0.03648216913554991 0.0101383207519854 0.01317286792061045 0.01000009289390658 0.001659408320147548 0.374844521015774 0.2632166216030947 0.01000000761864417 0.01548325969963793 0.09002192006929022 0.1054631318706765 0.009999999999999787 0.01142542618035813 0.01390085551421372 0.03617799963866819 0.01246822435853234 0.01475351445516537 0.009999999999999787 0.025760853112065 0.456419053652191 0.4379911361280175 0.01000056997295884 0.02944145474145721 0.01068294694368177 0.01236072877285377 0.01007450916286912 0.01632214916645269 0.1355155671671753 0.1273815814781862 0.01000022963983005 0.0248361252102951 0.07545945497587236 -0.1336719878805868 -0.221256388801262 0.01160734266257157 -0.07928310144156397 -0.1313700365824343 -0.07454248240268668 0.01000002626897001 0.02088773743413064 0.01000000000000001 0.01000000000000001 0.01000011713290128 0.490160107202489 0.0100000217207572 0.1172870075596658 0.01000000000000001 0.01000009201232788 0.01000000000000001 0.01000000000000001 0.5195369562133655 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1271903025880917 0.01000000000000001 0.09114494706360909 0.3410828915653125 -0.1071516000159011 0.0351970351661712 0.08784829666383992 1.753896482357001 0.08874240068764747 1.110803258130511 0.1619890934782718 -0.6045744211177879 -0.2694303288177547 0.04989968959241943 2.097832282794151 0.2195226201575405 -0.1688207078058503 0.1086343679824632 -0.6759495483644176 0.1830699604582158 +0.6477227193603352 -0.412197274367303 0.001337419980843357 -0.02543902767168316 1.726932177574833 0.9752216618188281 0.4076433022228185 0.008901451922794568 -0.009503446349308664 0.7955387410947106 0.2077908136640079 0.3363553237210248 0.3363553237210248 0.6691827949851343 0.6691827949851343 -0.6292819315026756 -0.2134005765314821 -0.09801322562507586 -0.1889984227300636 2.525885774244779 2.536185929799643 0.001595794894166946 -0.04558368758449971 1.190407121525378 -0.04823617536104496 0.860309377240795 2.78772504559455 -0.1691018163822195 -1.226731616920064 -0.4350663491582751 -0.7378598691170346 -2.127956726787023 0.4001287563800244 -2.596246313026248 -2.596246313026245 6.014720875280206 6.014720875280201 0.3804087095965691 -1.211433807713961 -0.2235623189601696 2.031251669892367 -0.1810783407938694 -0.21528583164568 -0.0135750482819903 1.411442683314951 0.03198055076250927 -0.09995010374976587 -0.2137655472463109 0.09283358480659398 -0.06388424715308627 -0.0855144921681612 -0.06927837648353119 0.01000036777533886 0.05240154309470624 0.01701856262822377 0.02916678276405538 0.01000000268727197 0.03625229671967922 0.01006670441785218 0.01316706148244196 0.01000009233094845 0.002333466901676129 0.3789598593529813 0.2664499366865423 0.01000001076950907 0.01626186229841275 0.09697810485527247 0.1110727681007844 0.009999999999999787 0.01273298991448435 0.01189212129278161 0.03251019748964135 0.01119458557228725 0.01274705706310053 0.009999999999999787 0.02613972988775037 0.4591547216557572 0.4404942099672522 0.01000027090174438 0.0309895662839792 0.01032963099647954 0.01120231990392684 0.0100359245503312 0.01690602042469846 0.1344451538554789 0.1231573433965116 0.0100001057689818 0.02608856461948328 -0.1803201216495383 -0.1054187654706842 -0.1870093596817806 0.2384178639620064 -0.08178658451841098 -0.06942831941581656 -0.1208955907404824 0.01000003521110793 0.01708761724329511 0.01000001321245902 0.01000000000000001 0.01000007059158392 0.2921115222801922 0.01000000698704118 0.09494417247246789 0.01000000000000001 0.01000005424229422 0.01000000000000001 0.01000000000000001 0.4151384578700784 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1380381862075983 0.01000000000000001 -0.02653952149575197 0.183598806255707 0.002079613637840236 -0.01485738001581184 0.1039916068561611 -1.106282597493332 0.116473155700475 0.423776159344226 0.2059426149639553 -0.4601777045196003 -0.2945639579858091 0.05258744926573935 -1.454434290535295 0.2247575027476746 -0.1624233515755278 0.06516053391766218 -0.4870893270902478 0.175681658286667 +0.6546874797835646 -0.4031248522770245 0.0009562013664683455 -0.01932348388293859 1.746268683621556 0.9738642850314614 0.398013077167831 0.005883046178770002 -0.01469107802920711 0.7810120797263362 0.2113605877024587 0.3236910017685473 0.3236910017685473 0.7132515245129056 0.7132515245129056 -0.6253671809651231 -0.2218436240557944 -0.09875559530973721 -0.1749472691710769 2.523434329015034 2.533233746028161 0.001413902396849132 -0.03676206353505584 1.427300596524423 -0.06151508208948986 0.8955847263782843 2.765390192873761 -0.2192337700739087 -1.553131161402904 -0.4306604827758735 -0.7529736309628272 -2.066405766774027 0.5995977986316237 -1.073650356687569 -1.073650356687569 6.608519870282743 6.608519870282743 0.7578063315826942 -1.209973323964164 -0.04612182738264092 1.987987667424205 -0.5286137104720523 -0.6419217523779164 -0.04474595817586913 1.125542548692044 -0.03023417966146447 -0.09829449287176395 -0.2057365679223047 0.1403046986674426 -0.0673476621737783 -0.07682037625830906 -0.082923988012789 0.01000020256880729 0.05209667800512507 0.01592756064471157 0.02920418517669043 0.01000001214736379 0.03596702439588739 0.01003302853998278 0.01327245913907227 0.01000006740102188 0.003083965184831783 0.3111072038976599 0.2438943258761905 0.01000000444214599 0.01684981892521042 0.09034266028743332 0.1103035417851466 0.009999999999999787 0.01418896559494742 0.01093803354225198 0.02960766638172529 0.01059195019344994 0.01082003905667817 0.009999999999999787 0.02640546071736338 0.4261039692689623 0.416681493745819 0.01000013006666922 0.03256838212489077 0.01016325322037925 0.01011781375650944 0.01001778451974022 0.01731519885799759 0.1385520710124988 0.121293038611292 0.01000004751340189 0.02727076704696119 -0.436099698274949 -0.07716554306078177 -0.1527623305622992 0.4652283852614412 -0.08429006759525792 -0.007486602249198782 -0.1672486990782781 0.01000004415324596 0.01328749705245968 0.01000002704173519 0.01000000000000001 0.01000002405026656 0.09406293735789528 0.01000000000000001 0.07260133738527008 0.01000000000000001 0.01000001647226068 0.01000000000000001 0.01000000000000001 0.3107399595267915 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.1488860698271048 0.01000000000000001 -0.04436181546036812 -0.2126529435757505 -0.1230602327886766 0.06713039339896022 0.1097987147624877 -5.651649243954514 0.03399651988103491 -0.7209503583998003 0.2046070551354112 -0.3848192550594027 -0.2466185216934349 0.01740870951603073 -5.459007712042615 0.2283396638240626 -0.147598808695299 0.05846959150626962 0.00172869268775483 0.1629007148726334 +0.6616522402067939 -0.3923372211246332 0.0004973831180503652 -0.01296691074206047 1.765451459094162 0.9721751100998439 0.386033561676272 0.002906585690134023 -0.02002096493722316 0.7665167589265174 0.2159656996864161 0.3203202406988312 0.3203202406988312 0.7607044905876843 0.7607044905876843 -0.6183754835511832 -0.2302124954191025 -0.1000736256550421 -0.1620863843744638 2.518461044077259 2.527054969197153 0.001167013218858592 -0.02966443391499718 1.608662051684376 -0.06944240089962239 0.9302095964045325 2.740690632543697 -0.2711852294686095 -1.815779394254978 -0.4232017055921675 -0.7768954770329883 -2.030590225290443 0.7998752497890771 0.08501101085874252 0.08501101085874296 7.034531771635912 7.034531771635912 1.247777718638585 -1.198848220928946 -0.2412624695309082 1.662386221951814 -0.8294008762905274 -1.146642668418326 -0.022647926183887 0.9163709479327169 -0.06059450270440347 -0.1053334092653193 -0.2055581788454832 0.1923925799189035 -0.02556253118235707 -0.06226508567664313 -0.1024194094765565 0.01000011920110788 0.05155112484430946 0.01458432612793548 0.02702246587301049 0.01000001586642973 0.03563752534540665 0.01001592277310959 0.01345318950031871 0.01000004608265437 0.003792891022802891 0.281368779026903 0.2040757526406853 0.009999999999999787 0.01668664224711147 0.1037186391293354 0.1119244030852542 0.009999999999999787 0.01541536560926637 0.01045294110245498 0.02707154084052021 0.0992739987281217 0.01807278232517184 0.009999999999999787 0.02667909843677974 0.4156764689762915 0.3897888144265642 0.01000005855813768 0.03410465877239854 0.01007873004339821 0.009207814192397912 0.0100085702678494 0.01747608868560224 0.2061082900153641 0.1331879329103742 0.01000001791750416 0.02839984059680045 0.02529811364089452 -0.1938988191461106 -0.2536897162973165 0.3946823586491192 0.3980561751166899 0.0144673251450107 -0.213236207480982 0.01000003942329564 0.01332624762754575 0.01000001286846708 0.01000000000000001 0.01000002806256584 0.3418987695221065 0.01000000000000001 0.1604197350098027 0.01000000000000001 0.01000001858966704 0.3601896365056652 0.01000000000000001 0.4685153375572817 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.438358759066542 0.01000000000000001 -0.1069099671211645 -0.3570714336146851 0.003964143563223698 -0.007765537217793946 0.09225132820424913 -5.149408317982721 -0.04231815746483553 1.178154440218807 0.1571743085993183 -0.3523679712644325 2.015881663269204 0.06720600311110549 -2.100927485508622 0.2076993973983145 -0.1066882550303244 -0.01184900556313402 3.57869946699919 0.1666945283518758 +0.6686170006300234 -0.3815779803062549 0 -0.006359962401108366 1.784411156670708 0.9700122027731819 0.3737108923951569 0 -0.02550365577217617 0.7536390229734087 0.2235760077366384 0.324586936318175 0.324586936318175 0.8114684894265207 0.8114684894265207 -0.6080162000402711 -0.2386212498125841 -0.1008472700848131 -0.152386605636023 2.512857819473362 2.517062825013944 0.001147219831816315 -0.02394509717485338 1.419148030660006 -0.07258936671325866 0.9674824471514463 2.701331163558203 -0.3552707236111337 -1.651671173887599 -0.4105952416897347 -0.7968527933910838 -1.601900242266194 1.462569640795185 1.119521044749527 1.119521044749527 7.5593157520969 7.5593157520969 1.724842063566951 -1.221426131452406 0.1102039849331984 1.080244725480482 -0.7095071304475344 -1.73696799100674 0.02046684948361133 0.7297520846527723 -0.001953636701200079 -0.1322254358571739 -0.2236784889423014 0.2222854778214978 0.09595785989598138 -0.04637074765099625 -0.1267095197725938 0.01000007750874232 0.05068254258367366 0.01396454984184725 0.02502124082685864 0.01000001010315366 0.03568128776058765 0.0100078828185528 0.01326829148324205 0.01000003818867956 0.00434773891782303 0.3510651746588573 0.1809862718297572 0.009999999999999787 0.01679713755674372 0.1494945936072893 0.1265998572167124 0.009999999999999787 0.01651216939385414 0.01022435431045476 0.02457518670953229 0.2871254515724271 0.03453431944869978 0.009999999999999787 0.02742569966449127 0.4613688538923459 0.3896810228763155 0.01000002519298659 0.03539024381380029 0.01003898382910373 0.008729598785322423 0.01000423918461024 0.01715599100121867 0.352799565026769 0.1734366789356621 0.01000000401993262 0.02966755156661716 0.4866959255567381 -0.3106320952314395 -0.3546171020323338 0.3241363320367971 0.8804024178286379 0.03642125253922013 -0.2592237158836859 0.01000003469334565 0.01336499820263204 0.01000000000000001 0.01000000000000001 0.010000032074865 0.5897346016863175 0.01000000000000001 0.2482381326343349 0.01000000000000001 0.01000002070707351 0.7103792812005009 0.01000000000000001 0.6262907155877721 0.01000000000000001 0.01000000000000001 0.01000000000000001 0.7278314483059795 0.01000000000000001 -0.1371228237907607 -0.1608222280578646 -0.0158745026378747 -0.03786342005924595 0.06555370600618407 -0.8477245120820593 0.1125841441650776 3.027805388900565 0.1673922130283307 -0.3733985003142031 2.397777924281034 0.1532248696036318 2.232538340334479 0.1563510839232331 -0.02360770755017705 -0.07965074079480237 8.143818299012249 0.2027118243888117 diff --git a/OpenSim/Moco/Test/subject_20dof18musc_running.osim b/OpenSim/Moco/Test/subject_20dof18musc_running.osim new file mode 100644 index 0000000000..bd4b0b47e8 --- /dev/null +++ b/OpenSim/Moco/Test/subject_20dof18musc_running.osim @@ -0,0 +1,6101 @@ + + + + + + + 9.9999999999999995e-08 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + + + + + + 0 -9.8066499999999994 0 + + Arnold, E.M., Hamner, S.R., Ward, S.R., Lieber, R. L., and Delp, S.L. + + + + meters + + N + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.1174152876956673 0.93144510857501561 1.1575839492192117 + + + + 1 + + 1 1 1 + + + r_pelvis.vtp + + + + .. + + 1.1174152876956673 0.93144510857501561 1.1575839492192117 + + + + 1 + + 1 1 1 + + + l_pelvis.vtp + + + + .. + + 1.1174152876956673 0.93144510857501561 1.1575839492192117 + + + + 1 + + 1 1 1 + + + sacrum.vtp + + + + + + + + true + + -0.59999999999999998 0.45000000000000001 0 + + -0.086041147732501516 -0.092212851873723162 0.070612562895559478 + + -x + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.042441737873665113 + + 0.1642763015883415 + + + + true + + -0.75 0.39000000000000001 0 + + -0.089393402573479844 -0.077309748734366676 0.078715631204489808 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.043040627138763748 + + 0.10660022437197854 + + + + true + + -0.10000000000000001 0 0 + + -0.092745208519892042 -0.081966927782632315 0.078715631204489808 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.041102710446590321 + + 0.1155418151936493 + + + + true + + 0.59999999999999998 -0.45000000000000001 0 + + -0.086041147732501516 -0.092212851873723162 -0.070612562895559478 + + -x + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.042441737873665113 + + 0.1642763015883415 + + + + true + + 0.75 -0.39000000000000001 0 + + -0.089393402573479844 -0.077309748734366676 -0.078715631204489808 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.043040627138763748 + + 0.10660022437197854 + + + + true + + 0.10000000000000001 0 0 + + -0.092745208519892042 -0.081966927782632315 -0.078715631204489808 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.041102710446590321 + + 0.1155418151936493 + + + + true + + -0.25 -0.27000000000000002 0.10000000000000001 + + -0.082688892891523202 -0.05588652053606321 0.075937394922276219 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.051749107126148219 + + 0.11428890247605485 + + + + true + + 0.25 0.27000000000000002 0.10000000000000001 + + -0.082688892891523202 -0.05588652053606321 -0.075937394922276219 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.051749107126148219 + + 0.11428890247605485 + + + + true + + -0.32000000000000001 -0.23999999999999999 0.90000000000000002 + + -0.079336638050544875 -0.060543792573547715 0.087513234414468344 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.057261606045311809 + + 0.11354388705201972 + + + + true + + 0.32000000000000001 0.23999999999999999 0.90000000000000002 + + -0.079336638050544875 -0.060543792573547715 -0.087513234414468344 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.057261606045311809 + + 0.11354388705201972 + + + + + + 11.486705868298447 + + -0.07900140358855573 0 0 + + 0.11593744392262688 0.11593744392262688 0.054122743858825761 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95835596996037176 1.0062957941873822 0.95835596996037176 + + + + 1 + + 1 1 1 + + + femur_r.vtp + + + + + + + + true + + 0 0 0 + + 0.0047917798498018586 -0.41258127561682673 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.024558147051846925 + + 0.095835596996037165 + + + + true + + -0.062336599999999999 0.050760100000000002 0 + + 0.0034388473190570721 -0.40526724887890736 0.0020040243710879057 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.024555882805153823 + + 0.095854615931990128 + + + + true + + 1.66157 0.186644 0 + + 0.014033612618989643 -0.11330386444239685 0.022392017249271371 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.015830036364277732 + + 0.070301778408725674 + + + + true + + 1.77711 0.136489 0 + + 0.029452839908117191 -0.23336923830774167 0.014484280014049297 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.019292412210096827 + + 0.10034979890329535 + + + + true + + 1.6112599999999999 0.18656 0 + + 0.0049671449871025401 -0.073353711330528495 0.024345142714564304 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.020239949323242808 + + 0.07032273232922924 + + + + true + + 1.6113900000000001 0.13655999999999999 0 + + 0.022054191768002458 -0.16172265842263417 0.019727027370383655 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.020519407389617595 + + 0.12064228819292078 + + + + true + + 1.7118800000000001 0.186636 0 + + 0.030290250954519933 -0.26237744775688443 0.0089746235174799401 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.020920806564767299 + + 0.2007575854871822 + + + + true + + 1.7111499999999999 -0.46336300000000002 0 + + -0.021707863128336197 -0.37920334747893708 -0.0030230128622792778 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.038544994805186049 + + 0.23907911270594023 + + + + true + + 1.8129500000000001 0.27634399999999998 0 + + 0.0058322963373121737 -0.085034935765643541 0.029172835903950273 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.01442231097298481 + + 0.050015180937333537 + + + + + + 9.0721275336156211 + + 0 -0.17107028501185498 0 + + 0.12029170533096845 0.028952795937718514 0.12029170533096845 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.90116027734621973 1.0021803384243346 0.90116027734621973 + + + + 1 + + 1 1 1 + + + tibia_r.vtp + + + + .. + + 0.90116027734621973 1.0021803384243346 0.90116027734621973 + + + + 1 + + 1 1 1 + + + fibula_r.vtp + + + + + + + + true + + 2.9672299999999998 -0.279725 -1.4781200000000001 + + -0.0066685860523620268 -0.074161345043400759 -0.002973828915242525 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.05227388017601823 + + 0.090412132910025408 + + + + true + + 2.9672299999999998 0.027972500000000001 -1.4781200000000001 + + -0.0066685860523620268 -0.074161345043400759 -0.002973828915242525 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.052264550949575515 + + 0.090436272865069875 + + + + true + + 0 -0.40000000000000002 0 + + -0.0027034808320386592 -0.020043606768486693 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.03426013108386998 + + 0.09011602773462199 + + + + true + + 0 -0.10000000000000001 0 + + -0.0009011602773462198 -0.020043606768486693 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.033498794837561761 + + 0.090116027734621976 + + + + true + + 0 -0.20000000000000001 0 + + -0.0018023205546924396 -0.020544696937698857 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.040445988085124285 + + 0.09011602773462199 + + + + true + + 0 0 0 + + -0.052267296086080751 -0.060130820305460075 0 + + y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.028550109236558315 + + 0.13517404160193297 + + + + + + 3.6161129325563808 + + 0 -0.18710680751793854 0 + + 0.045022046721427809 0.0037196674802111402 0.045022046721427809 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95835596996037176 0.95835596996037176 0.95835596996037176 + + + + 1 + + 1 1 1 + + + r_patella.vtp + + + + + + + + + 0.084075235276159149 + + 0.0017250407459286693 0.025300597606953815 0 + + 2.367363619686778e-06 1.0813987365959002e-05 1.0813987365959002e-05 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.94359429069757828 1.66076 1.1694794680047496 + + + + false + + 1 + + 1 1 1 + + + r_talus.vtp + + + + + + + + + 0.097535075726402728 + + 0 0 0 + + 0.0019309034640562972 0.0010981825213608272 0.0019309034640562972 0 0 0 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + .. + + 0.1 -0.01 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.94359429069757828 1.66076 1.1694794680047496 + + + + 1 + + 1 1 1 + + + r_foot.vtp + + + + + + + + + 1.219188446580034 + + 0.094359429069757833 0.0498228 0 + + 0.0024420616188596581 0.0037798946060735566 0.0037798946060735566 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.94359429069757828 1.66076 1.1694794680047496 + + + + 1 + + 1 1 1 + + + r_bofoot.vtp + + + + + + + + + 0.21126097402338828 + + 0.032648250200163979 0.0099645700000000007 -0.020465832119663758 + + 0.00017443323898193438 0.00020716338536110352 0.00020716338536110352 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95835597359529134 1.0224540406772964 0.95835597359529134 + + + + 1 + + 1 1 1 + + + femur_l.vtp + + + + + + + + true + + 0 0 0 + + 0.0047917798679764567 -0.41920615667769151 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.024760125178407342 + + 0.095835597359529137 + + + + true + + 0.062336599999999999 -0.050760100000000002 0 + + 0.003438847332100173 -0.40526724887890736 -0.0020040243786889093 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.024757121147057679 + + 0.095861234935227996 + + + + true + + -1.66157 -0.186644 0 + + 0.014033612672217312 -0.11512320203726227 -0.022392017334201382 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.015836003950806962 + + 0.071387406118179259 + + + + true + + -1.77711 -0.136489 0 + + 0.029452840019827984 -0.23711648409523409 -0.014484280068986288 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.019302652568839385 + + 0.1018740481862539 + + + + true + + -1.6112599999999999 -0.18656 0 + + 0.0049671450059422754 -0.07453156316641521 -0.024345142806902264 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.020246429122108462 + + 0.071415222193391595 + + + + true + + -1.6113900000000001 -0.13655999999999999 0 + + 0.022054191851651139 -0.16431946404667766 -0.019727027445205707 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.020523091002776151 + + 0.12254426631226301 + + + + true + + -1.7118800000000001 -0.186636 0 + + 0.030290251069406918 -0.26659048282941383 -0.0089746235515195203 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.020930767185067738 + + 0.20382509360206721 + + + + true + + -1.7111499999999999 0.46336300000000002 0 + + -0.021707863210671301 -0.38529227400904664 0.0030230128737451724 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.038617900852470077 + + 0.24215745346312617 + + + + true + + -1.8129500000000001 -0.27634399999999998 0 + + 0.0058322963594333154 -0.086400354820648978 -0.029172836014599048 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.014438623196989823 + + 0.050724832738423188 + + + + + + 9.0721275336156211 + + 0 -0.17381718691514039 0 + + 0.12371717339541859 0.028952796157346895 0.12371717339541859 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.92299156705002372 0.99458208915871027 0.92299156705002372 + + + + 1 + + 1 1 1 + + + tibia_l.vtp + + + + .. + + 0.92299156705002372 0.99458208915871027 0.92299156705002372 + + + + 1 + + 1 1 1 + + + fibula_l.vtp + + + + + + + + true + + -2.9672299999999998 0.279725 -1.4781200000000001 + + -0.0068301375961701757 -0.073599074597744563 0.0030458721712650783 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.052683172020955633 + + 0.092505676209053311 + + + + true + + -2.9672299999999998 -0.027972500000000001 -1.4781200000000001 + + -0.0068301375961701757 -0.073599074597744563 0.0030458721712650783 + + -y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.052677201569395066 + + 0.092522522203917421 + + + + true + + 0 0.40000000000000002 0 + + -0.0027689747011500712 -0.019891641783174204 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.034516325811757211 + + 0.092299156705002383 + + + + true + + 0 0.10000000000000001 0 + + -0.00092299156705002376 -0.019891641783174204 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.033749296349273719 + + 0.092299156705002383 + + + + true + + 0 0.20000000000000001 0 + + -0.0018459831341000475 -0.020388932827753559 0 + + all + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.040748440194435596 + + 0.092299156705002369 + + + + true + + 0 0 0 + + -0.053533510888901382 -0.059674925349522616 0 + + y + + + + false + + 0.5 + + 0 1 1 + + + + 3 + + + + 0.028763604843131008 + + 0.13844873505750355 + + + + + + 3.6161129325563808 + + 0 -0.18568821636392357 0 + + 0.044461243562004316 0.0039020740251826812 0.044461243562004316 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95835597359529134 0.95835597359529134 0.95835597359529134 + + + + 1 + + 1 1 1 + + + l_pat.vtp + + + + + + + + + 0.084075235276159149 + + 0.0017250407524715245 0.025300597702915692 0 + + 2.3673636376449827e-06 1.0813987447991099e-05 1.0813987447991099e-05 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95171272681783847 1.66076 1.2378939239920357 + + + + false + + 1 + + 1 1 1 + + + l_talus.vtp + + + + + + + + + 0.097535075726402728 + + 0 0 0 + + 0.0019680258628545651 0.0011724273189573633 0.0019680258628545651 0 0 0 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + .. + + 0.1 -0.01 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95171272681783847 1.66076 1.2378939239920357 + + + + 1 + + 1 1 1 + + + l_foot.vtp + + + + + + + + + 1.219188446580034 + + 0.095171272681783853 0.0498228 0 + + 0.002584922029591586 0.0038955458063421123 0.0038955458063421123 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.95171272681783847 1.66076 1.2378939239920357 + + + + 1 + + 1 1 1 + + + l_bofoot.vtp + + + + + + + + + 0.21126097402338828 + + 0.032929147124087031 0.0099645700000000007 0.021663081673076387 + + 0.00018463756960725909 0.00021433841024881745 0.00021433841024881745 0 0 0 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + .. + + 0 0.55000000000000004 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.1930520707910313 1.0248948455465248 1.2101299619766135 + + + + 1 + + 1 1 1 + + + hat_spine.vtp + + + + .. + + 1.1930520707910313 1.0248948455465248 1.2101299619766135 + + + + 1 + + 1 1 1 + + + hat_jaw.vtp + + + + .. + + 1.1930520707910313 1.0248948455465248 1.2101299619766135 + + + + 1 + + 1 1 1 + + + hat_skull.vtp + + + + .. + + 1.1930520707910313 1.0248948455465248 1.2101299619766135 + + + + 1 + + 1 1 1 + + + hat_ribs_scap.vtp + + + + + + + + + 26.165344624819152 + + -0.014499250654648294 0.32796617654090188 -0.0023056099275556344 + + 1.5244611513558659 0.97961329298266209 1.5244611513558659 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.0969238885932415 1.0969238885932415 1.0969238885932415 + + + + 1 + + 1 1 1 + + + humerus_rv.vtp + + + + + + + + + 1.9824004141391354 + + 0 -0.18044617352136544 0 + + 0.014019622671032194 0.0048363355957913665 0.015736574618773706 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.0142348369837353 1.0142348369837353 1.0142348369837353 + + + + 1 + + 1 1 1 + + + ulna_rv.vtp + + + + + + + + + 0.59252558503789654 + + 0 -0.12224065372746469 0 + + 0.0029718229138359013 0.00062004948033443186 0.0032236553079523136 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.0142348369837353 1.0142348369837353 1.0142348369837353 + + + + 1 + + 1 1 1 + + + radius_rv.vtp + + + + + + + + + 0.59252558503789654 + + 0 -0.12224065372746469 0 + + 0.0029718229138359013 0.00062004948033443186 0.0032236553079523136 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + pisiform_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + lunate_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + scaphoid_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + triquetrum_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + hamate_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + capitate_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + trapezoid_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + trapezium_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal2_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + index_proximal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + index_medial_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + index_distal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal3_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + middle_proximal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + middle_medial_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + middle_distal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal4_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + ring_proximal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + ring_medial_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + ring_distal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal5_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + little_proximal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + little_medial_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + little_distal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal1_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + thumb_proximal_rvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + thumb_distal_rvs.vtp + + + + + + + + + 0.44622297144829248 + + 0 -0.068095000000000003 0 + + 0.00087001287547951221 0.0005335168642234228 0.0013069700147337964 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.0605025325609003 1.0605025325609003 1.0605025325609003 + + + + 1 + + 1 1 1 + + + humerus_lv.vtp + + + + + + + + + 1.9824004141391354 + + 0 -0.17445478761133323 0 + + 0.013104086640837053 0.0045205040220064868 0.014708914931105313 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.0162726191746843 1.0162726191746843 1.0162726191746843 + + + + 1 + + 1 1 1 + + + ulna_lv.vtp + + + + + + + + + 0.59252558503789654 + + 0 -0.12248625742602881 0 + + 0.0029837767756387768 0.00062254356763834034 0.0032366221404886535 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.0162726191746843 1.0162726191746843 1.0162726191746843 + + + + 1 + + 1 1 1 + + + radius_lv.vtp + + + + + + + + + 0.59252558503789654 + + 0 -0.12248625742602881 0 + + 0.0029837767756387768 0.00062254356763834034 0.0032366221404886535 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + pisiform_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + lunate_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + scaphoid_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + triquetrum_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + hamate_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + capitate_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + trapezoid_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + trapezium_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal2_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + index_proximal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + index_medial_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + index_distal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal3_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + middle_proximal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + middle_medial_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + middle_distal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal4_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + ring_proximal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + ring_medial_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + ring_distal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal5_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + little_proximal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + little_medial_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + little_distal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + metacarpal1_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + thumb_proximal_lvs.vtp + + + + .. + + 0.84999999999999998 0.84999999999999998 0.84999999999999998 + + + + 1 + + 1 1 1 + + + thumb_distal_lvs.vtp + + + + + + + + + 0.44622297144829248 + + 0 -0.068095000000000003 0 + + 0.00087001287547951221 0.0005335168642234228 0.0013069700147337964 0 0 0 + + + + + + + + + + ground_offset + + pelvis_offset + + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + 0 + + 0 + + -5 5 + + true + + false + + + + false + + + + 0.93000000000000005 + + 0 + + -1 2 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /ground + + 0 0 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + 0 0 0 + + 0 0 0 + + + + + + + + pelvis_tilt + + 0 0 1 + + + 1 0 + + + + + pelvis_list + + 1 0 0 + + + 1 0 + + + + + pelvis_rotation + + 0 1 0 + + + 1 0 + + + + + + pelvis_tx + + 1 0 0 + + + 1 0 + + + + + pelvis_ty + + 0 1 0 + + + 1 0 + + + + + + + 0 0 1 + + + + + + pelvis_offset + + torso_offset + + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + -0.11252350310377283 0.075912585720965098 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/torso + + 0 0 0 + + 0 0 0 + + + + + + + + lumbar_extension + + 0 0 1 + + + 1 0 + + + + + lumbar_bending + + 1 0 0 + + + 1 0 + + + + + lumbar_rotation + + 0 1 0 + + + 1 0 + + + + + + + + 1 0 0 + + + + + 0 + + + 1.1174170254914513 + + + + + + + 0 1 0 + + + + + 0 + + + 0.93144274453235665 + + + + + + + 0 0 1 + + + + + 0 + + + 1.1575822105091809 + + + + + + + pelvis_offset + + femur_r_offset + + + + + 0 + + 0 + + -1.0467 2.0943950999999998 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + -0.062883754412587575 -0.07310896077227573 0.089434806754840585 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_r + + 0 0 0 + + 0 0 0 + + + + + + femur_r_offset + + tibia_r_offset + + + + + 0 + + 0 + + 0 2.4430000000000001 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_r + + -0.0077530997969794081 -0.41052876832889129 -0.0026354789173910222 + + -1.64157 1.44618 1.5708 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_r + + -0.007289970964925876 -0.0035425112469164933 -0.0013379916434676507 + + -1.64157 1.44618 1.5708 + + + + + + + + knee_angle_r + + 1 0 0 + + + 1 0 + + + + + knee_angle_r + + 0 0 1 + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 0.0126809 0.0226969 0.0296054 0.0332049 0.0335354 0.0308779 0.0257548 0.0189295 0.011407 0.00443314 -0.00050475 -0.0016782 0.0027 0.0143 + + + + + knee_angle_r + + 0 1 0 + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 0.059461 0.109399 0.150618 0.18392 0.210107 0.229983 0.24435 0.254012 0.25977 0.262428 0.262788 0.261654 0.2598 0.2581 + + + + + + knee_angle_r + + 0 1 0 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 0.000479 0.000835 0.001086 0.001251 0.001346 0.001391 0.001403 0.0014 0.0014 0.001421 0.001481 0.001599 0.0018 0.0021 + + + 1.0062957941873822 + + + + + knee_angle_r + + 0 0 1 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 0.000988 0.001899 0.002734 0.003492 0.004173 0.004777 0.005305 0.005756 0.00613 0.006427 0.006648 0.006792 0.0069 0.0068 + + + 0.95835596996037176 + + + + + + + 1 0 0 + + + + + 0 + + + 0.95835596996037176 + + + + + + + femur_r_offset + + patella_r_offset + + + + + 0 + + 0 + + -99999.899999999994 99999.899999999994 + + false + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_r + + -0.0077530997969794081 -0.41052876832889129 -0.0026354789173910222 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/patella_r + + 0 0 0 + + 0 0 0 + + + + + + + + knee_angle_r_beta + + 0 0 1 + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0.00113686 -0.00629212 -0.105582 -0.253683 -0.414245 -0.579047 -0.747244 -0.91799 -1.09044 -1.26379 -1.43763 -1.61186 -1.78634 -1.9619 -2.1348 + + + + + + + 0 1 0 + + + 0 + + + + + + + 1 0 0 + + + 0 + + + + + + knee_angle_r_beta + + 1 0 0 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0.0524 0.0488 0.0437 0.0371 0.0296 0.0216 0.0136 0.0057 -0.0019 -0.0088 -0.0148 -0.0196 -0.0227 -0.0236 -0.0216 + + + 0.95835596996037176 + + + + + knee_angle_r_beta + + 0 1 0 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + -0.0108 -0.019 -0.0263 -0.0322 -0.0367 -0.0395 -0.0408 -0.0404 -0.0384 -0.0349 -0.0301 -0.0245 -0.0187 -0.0133 -0.0089 + + + 1.0062957941873822 + + + + + + + 0 0 1 + + + + + 0.0027499999999999998 + + + 0.95835596996037176 + + + + + + + tibia_r_offset + + talus_r_offset + + + + + 0 + + 0 + + -0.69813170000000002 0.52359878000000004 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_r + + -0.0090116027734621976 -0.40087213536973382 0 + + 0.175895 -0.105208 0.0186622 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_r + + 0 0 0 + + 0.175895 -0.105208 0.0186622 + + + + + + talus_r_offset + + calcn_r_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_r + + -0.046018917576440901 -0.069668900000000006 0.0092622480232940445 + + -1.7681899999999999 0.906223 1.8196000000000001 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_r + + 0 0 0 + + -1.7681899999999999 0.906223 1.8196000000000001 + + + + + + calcn_r_offset + + toes_r_offset + + + + + 0 + + 0 + + -1.0467 0.52359878000000004 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_r + + 0.16871382714705047 -0.0033215200000000001 0.0012630359511917101 + + -3.1415899999999999 0.61990100000000004 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/toes_r + + 0 0 0 + + -3.1415899999999999 0.61990100000000004 0 + + + + + + pelvis_offset + + femur_l_offset + + + + + 0 + + 0 + + -1.0467 2.0943950999999998 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + -0.062883754412587575 -0.07310896077227573 -0.089434806754840585 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_l + + 0 0 0 + + 0 0 0 + + + + + + femur_l_offset + + tibia_l_offset + + + + + 0 + + 0 + + 0 2.4430000000000001 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_l + + -0.007753099826385907 -0.41712069196424323 0.0026354789273870514 + + 1.64157 -1.44618 1.5708 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_l + + -0.007466576028496018 -0.0035156529236703305 0.0013704055036034195 + + 1.64157 -1.44618 1.5708 + + + + + + + + knee_angle_l + + -1 0 0 + + + 1 0 + + + + + knee_angle_l + + 0 0 1 + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 0.0126809 0.0226969 0.0296054 0.0332049 0.0335354 0.0308779 0.0257548 0.0189295 0.011407 0.00443314 -0.00050475 -0.0016782 0.0027 0.0143 + + + + + knee_angle_l + + 0 1 0 + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 -0.059461 -0.109399 -0.150618 -0.18392 -0.210107 -0.229983 -0.24435 -0.254012 -0.25977 -0.262428 -0.262788 -0.261654 -0.2598 -0.2581 + + + + + + knee_angle_l + + 0 1 0 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 0.000479 0.000835 0.001086 0.001251 0.001346 0.001391 0.001403 0.0014 0.0014 0.001421 0.001481 0.001599 0.0018 0.0021 + + + 1.0224540406772964 + + + + + knee_angle_l + + 0 0 1 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0 -0.000988 -0.001899 -0.002734 -0.003492 -0.004173 -0.004777 -0.005305 -0.005756 -0.00613 -0.006427 -0.006648 -0.006792 -0.0069 -0.0068 + + + 0.95835597359529134 + + + + + + + 1 0 0 + + + + + 0 + + + 0.95835597359529134 + + + + + + + femur_l_offset + + patella_l_offset + + + + + 0 + + 0 + + -99999.899999999994 99999.899999999994 + + false + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_l + + -0.007753099826385907 -0.41712069196424323 0.0026354789273870514 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/patella_l + + 0 0 0 + + 0 0 0 + + + + + + + + knee_angle_l_beta + + 0 0 1 + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0.00113686 -0.00629212 -0.105582 -0.253683 -0.414245 -0.579047 -0.747244 -0.91799 -1.09044 -1.26379 -1.43763 -1.61186 -1.78634 -1.9619 -2.1348 + + + + + + + 0 1 0 + + + 0 + + + + + + + 1 0 0 + + + 0 + + + + + + knee_angle_l_beta + + 1 0 0 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + 0.0524 0.0488 0.0437 0.0371 0.0296 0.0216 0.0136 0.0057 -0.0019 -0.0088 -0.0148 -0.0196 -0.0227 -0.0236 -0.0216 + + + 0.95835597359529134 + + + + + knee_angle_l_beta + + 0 1 0 + + + + + 0 0.174533 0.349066 0.523599 0.698132 0.872665 1.0472 1.22173 1.39626 1.5708 1.74533 1.91986 2.0944 2.27 2.443 + -0.0108 -0.019 -0.0263 -0.0322 -0.0367 -0.0395 -0.0408 -0.0404 -0.0384 -0.0349 -0.0301 -0.0245 -0.0187 -0.0133 -0.0089 + + + 1.0224540406772964 + + + + + + + 0 0 1 + + + + + -0.0027499999999999998 + + + 0.95835597359529134 + + + + + + + tibia_l_offset + + talus_l_offset + + + + + 0 + + 0 + + -0.69813170000000002 0.52359878000000004 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_l + + -0.0092299156705002369 -0.3978328356634841 0 + + -0.175895 0.105208 0.0186622 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_l + + 0 0 0 + + -0.175895 0.105208 0.0186622 + + + + + + talus_l_offset + + calcn_l_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_l + + -0.046414852191932962 -0.069668900000000006 -0.0098040887969624256 + + 1.7681899999999999 -0.906223 1.8196000000000001 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_l + + 0 0 0 + + 1.7681899999999999 -0.906223 1.8196000000000001 + + + + + + calcn_l_offset + + toes_l_offset + + + + + 0 + + 0 + + -1.0467 0.52359878000000004 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_l + + 0.17016539636678935 -0.0033215200000000001 -0.001336923454014303 + + -3.1415899999999999 -0.61990100000000004 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/toes_l + + 0 0 0 + + -3.1415899999999999 -0.61990100000000004 0 + + + + + + torso_offset + + humerus_r_offset + + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/torso + + 0.0037478039807104439 0.3784267018826768 0.21714626078273061 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/humerus_r + + 0 0 0 + + 0 0 0 + + + + + + humerus_r_offset + + ulna_r_offset + + + + + 0 + + 0 + + 0 2.7909999999999999 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/humerus_r + + 0.014417967591669566 -0.31401969235925303 -0.010524984711052152 + + -0.0228627 0.228018 0.0051688999999999997 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/ulna_r + + 0 0 0 + + -0.0228627 0.228018 0.0051688999999999997 + + + + + + ulna_r_offset + + radius_r_offset + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/ulna_r + + -0.006609996325305032 -0.012780767385646282 0.025629334644407777 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/radius_r + + 0 0 0 + + 0 -1.4835298641951802 0 + + + + + + radius_r_offset + + hand_r_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/radius_r + + -0.0086439925187614623 -0.23173898370094603 0.013373279320261852 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/hand_r + + 0 0 0 + + 0 0 0 + + + + + + torso_offset + + humerus_l_offset + + + + + 0 + + 0 + + -1.5707963300000001 1.5707963300000001 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/torso + + 0.0037478039807104439 0.3784267018826768 -0.21714626078273061 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/humerus_l + + 0 0 0 + + 0 0 0 + + + + + + humerus_l_offset + + ulna_l_offset + + + + + 0 + + 0 + + 0 2.7909999999999999 + + true + + false + + + + false + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/humerus_l + + 0.013939245287980474 -0.30359324150380662 0.010175521799921839 + + 0.0228627 -0.228018 0.0051688999999999997 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/ulna_l + + 0 0 0 + + 0.0228627 -0.228018 0.0051688999999999997 + + + + + + ulna_l_offset + + radius_l_offset + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/ulna_l + + -0.006609996325305032 -0.012780767385646282 -0.025629334644407777 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/radius_l + + 0 0 0 + + 0 1.4835298641951802 0 + + + + + + radius_l_offset + + hand_l_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/radius_l + + -0.0086439925187614623 -0.23173898370094603 -0.013373279320261852 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/hand_l + + 0 0 0 + + 0 0 0 + + + + + + + + + + + + + + + + + true + + + + 1 0 + + + + knee_angle_r + + knee_angle_r_beta + + 1 + + + + true + + + + 1 0 + + + + knee_angle_l + + knee_angle_l_beta + + 1 + + + + + + + + + + /contactgeometryset/heel_r + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/lateralMidfoot_r + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/medialToe_r + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/medialMidfoot_r + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/heel_l + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/lateralMidfoot_l + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/medialToe_l + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/medialMidfoot_l + + /contactgeometryset/floor + + 10067776 + + 1 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20999999999999999 + + 300 + + 50 + + + + + + + + + + /bodyset/torso + + -0.095031479701057228 0.42783099539898856 0.0038319928373423046 + + true + + + + /bodyset/torso + + 0 0.42579370494470764 0.18138099430086907 + + true + + + + /bodyset/torso + + 0 0.42579370494470764 -0.18138099430086907 + + true + + + + /bodyset/torso + + 0.054643100828107902 0.39217841244907287 0 + + true + + + + /bodyset/pelvis + + 0.013960800291039088 0.0095825907675585551 0.14399586597579006 + + true + + + + /bodyset/pelvis + + 0.013960800291039088 0.0095825907675585551 -0.14399586597579006 + + true + + + + /bodyset/pelvis + + -0.16538178806307843 0.02395647691889639 0.056248385146792992 + + true + + + + /bodyset/pelvis + + -0.16538178806307843 0.02395647691889639 -0.056248385146792992 + + true + + + + /bodyset/femur_r + + 0 0 0 + + true + + + + /bodyset/femur_l + + 0 0 0 + + true + + + + /bodyset/femur_r + + 0.072926002284815383 -0.13172077758492584 0.067470605323708133 + + false + + + + /bodyset/femur_r + + 0.075950300851327257 -0.24172041745798023 0.065744761461012879 + + false + + + + /bodyset/femur_r + + 0.026432432859126331 -0.27703971650161541 0.086162271731038353 + + false + + + + /bodyset/femur_r + + -0.038897617057028877 -0.13658826639939536 0.074266154007408325 + + false + + + + /bodyset/femur_r + + -0.033638419017546717 -0.25293849962552506 0.069967730482827561 + + false + + + + /bodyset/femur_r + + 0 -0.41027815792987404 0.059465130415628335 + + true + + + + /bodyset/femur_r + + 0.0054059209468753037 -0.4072315379452463 -0.059465130415628335 + + true + + + + /bodyset/tibia_r + + 0.029510906231608569 -0.086714574610505912 0.04607588982215538 + + false + + + + /bodyset/tibia_r + + 0.028217874169256141 -0.19924483668189175 0.060029727530847454 + + false + + + + /bodyset/tibia_r + + -0.039276361036196472 -0.082108978244288311 0.081334784845063957 + + false + + + + /bodyset/tibia_r + + -0.051702547782760178 -0.19835817801094602 0.085220881322180642 + + false + + + + /bodyset/tibia_r + + -0.019843771143768504 -0.39475792681365929 0.052585993530986537 + + true + + + + /bodyset/tibia_r + + 0.0059531313431305513 -0.38963120049140393 -0.037703165173160157 + + true + + + + /bodyset/calcn_r + + 0.18542026193884015 0.017999999999999999 0.0049852987147759112 + + true + + + + /bodyset/calcn_r + + 0.13662545616546118 0.0050000000000000001 0.064808883292086852 + + true + + + + /bodyset/calcn_r + + -0.02927688346402739 0.012 -0.0099705974295518224 + + true + + + + /bodyset/femur_l + + 0.081447726741272097 -0.19865055150513722 -0.059640442917897607 + + false + + + + /bodyset/femur_l + + 0.067136392042641768 -0.33843286349363566 -0.035819034423193119 + + false + + + + /bodyset/femur_l + + -0.027507598492494623 -0.19158430256289471 -0.083777900281715945 + + false + + + + /bodyset/femur_l + + -0.047242577424995269 -0.32780196813764939 -0.062012181684702239 + + false + + + + /bodyset/femur_l + + 0 -0.41686471367305061 -0.056478829145699495 + + true + + + + /bodyset/femur_l + + 0.0051344390132454084 -0.413769183621023 0.056478829145699495 + + true + + + + /bodyset/tibia_l + + -0.020184571855540564 -0.39217397975178891 -0.053489115417182487 + + true + + + + /bodyset/tibia_l + + 0.030689973607803234 -0.078130940725161746 -0.042280980732559037 + + false + + + + /bodyset/tibia_l + + -0.048728952013316285 -0.073346316550836066 -0.083528549987498146 + + false + + + + /bodyset/tibia_l + + -0.041278591657943453 -0.19590732217339479 -0.090069123659809569 + + false + + + + /bodyset/tibia_l + + 0.006055371556662169 -0.38708081118358389 0.038350686525527068 + + true + + + + /bodyset/calcn_l + + 0.18666094132867875 0.017999999999999999 -0.0051500304832903425 + + true + + + + /bodyset/calcn_l + + -0.029472780209791379 0.025000000000000001 0.010300060966580685 + + true + + + + /bodyset/calcn_l + + 0.13753964097902646 0.0050000000000000001 -0.066950396282774452 + + true + + + + /bodyset/pelvis + + 0.013960800291039088 0.0095825907675585551 0 + + true + + + + /bodyset/calcn_r + + 0.15577782341048363 -0.058329510804339416 0.073616479025618409 + + false + + + + /bodyset/calcn_l + + 0.15257418846818938 -0.057054411913156453 -0.071501100741835355 + + false + + + + /bodyset/calcn_r + + 0.1960825431238632 -0.059871008273865864 0.0110996511787394 + + false + + + + /bodyset/calcn_l + + 0.1988384747198767 -0.05550464216993381 -0.0089394329721291799 + + false + + + + /bodyset/talus_l + + 0 0 0 + + true + + + + /bodyset/talus_l + + 0.01095353008669897 -0.11087165679495237 0.007194702472642589 + + false + + + + /bodyset/femur_l + + 0.0025672195066227042 -0.41531694864703678 0 + + true + + + + /bodyset/talus_r + + 0 0 0 + + true + + + + /bodyset/talus_r + + 0.0086460643374560719 -0.11319306424426105 -0.0074680023282236441 + + false + + + + /bodyset/femur_r + + 0.0027029604734376518 -0.40875484793756017 0 + + true + + + + /bodyset/pelvis + + -0.060435230552193513 -0.075213754934567098 0 + + true + + + + /bodyset/pelvis + + -0.075710493886019664 0.016769533843227474 0 + + true + + + + /bodyset/pelvis + + -0.16538178806307843 0.02395647691889639 0 + + true + + + + /bodyset/calcn_r + + 0.13565652633812741 0.049387970242062568 -0.0451131149339834 + + false + + + + /bodyset/calcn_l + + 0.14433743446759872 0.051015086183767794 0.043646352252333265 + + false + + + + /bodyset/humerus_r + + 0.0053886715104309564 -0.30933692427884846 -0.045460913638858305 + + true + + + + /bodyset/humerus_r + + 0.011674999715853307 -0.30783194470369857 0.036931123788768394 + + true + + + + /bodyset/humerus_l + + 0.005908579255137731 -0.29956121087501009 0.043663964722876689 + + true + + + + /bodyset/humerus_l + + 0.016623165097385598 -0.29479531249368135 -0.034505995102947037 + + true + + + + /bodyset/radius_l + + -0.029430746914989269 -0.23277216205886556 -0.038405348787658992 + + true + + + + /bodyset/ulna_l + + -0.036725653402687068 -0.24927033175854738 -0.024418490357219727 + + true + + + + /bodyset/radius_r + + -0.022862171730901472 -0.23131045386185858 0.036183639197264347 + + true + + + + /bodyset/ulna_r + + -0.03715446478322517 -0.24560203694979643 0.029370922373760894 + + true + + + + /bodyset/radius_r + + -0.036897326361648108 -0.15006844660044782 0.025084751557154172 + + false + + + + /bodyset/radius_l + + -0.035286564984114865 -0.13747343583751642 -0.022932496193022844 + + false + + + + /bodyset/humerus_r + + 0.029535074785246418 -0.18788344445522576 -0.0098481663045338896 + + false + + + + /bodyset/humerus_l + + 0.036858691815596534 -0.18028704282556096 -0.010462660988175387 + + false + + + + /bodyset/torso + + -0.031690929999999999 0.41998068 0.15640846999999999 + + false + + + + /bodyset/torso + + -0.032871629999999999 0.43495948000000001 -0.15875701 + + false + + + + /bodyset/torso + + -0.049311800000000003 0.43901979000000002 -0.0044722099999999999 + + false + + + + /bodyset/torso + + 0.071515780000000001 0.37474361 0.022124040000000001 + + false + + + + /bodyset/torso + + 0.042035759999999998 0.39028315000000002 0.14596803 + + false + + + + /bodyset/torso + + -0.076289549999999998 0.38123267 0.15059102999999999 + + false + + + + /bodyset/torso + + 0.040639700000000001 0.40674895999999999 -0.13938650999999999 + + false + + + + /bodyset/torso + + -0.075232830000000001 0.38822842000000002 -0.1563223 + + false + + + + /bodyset/humerus_r + + -0.01735273 -0.045774210000000003 0.051302489999999999 + + false + + + + /bodyset/humerus_r + + 0.036176819999999998 -0.10438123000000001 0.01370967 + + false + + + + /bodyset/humerus_r + + -0.03406762 -0.15118471 0.023523800000000001 + + false + + + + /bodyset/humerus_r + + 0.012698620000000001 -0.25931268000000002 0.029617259999999999 + + false + + + + /bodyset/humerus_r + + -0.0032086200000000001 -0.25483159999999999 -0.03213622 + + false + + + + /bodyset/ulna_r + + 0.046069060000000002 -0.078569079999999999 0.045655439999999999 + + false + + + + /bodyset/radius_r + + -0.0023780799999999999 -0.25136192000000002 0.048137550000000001 + + false + + + + /bodyset/radius_r + + -0.022401319999999999 -0.23922856000000001 -0.01612622 + + false + + + + /bodyset/humerus_l + + -0.027235180000000001 -0.047854380000000002 -0.056330720000000001 + + false + + + + /bodyset/humerus_l + + -0.050967610000000003 -0.098604600000000001 -0.0016275599999999999 + + false + + + + /bodyset/humerus_l + + 0.022246200000000001 -0.14148561000000001 -0.039669240000000001 + + false + + + + /bodyset/humerus_l + + 0.011220259999999999 -0.26098436000000003 -0.028167279999999999 + + false + + + + /bodyset/humerus_l + + -0.0068752099999999997 -0.25270987 0.032905719999999999 + + false + + + + /bodyset/ulna_l + + 0.034067050000000001 -0.066677009999999995 -0.052491339999999997 + + false + + + + /bodyset/radius_l + + -0.0025403600000000002 -0.25230322999999999 -0.048637659999999999 + + false + + + + /bodyset/radius_l + + -0.023449169999999998 -0.24330192 0.016726669999999999 + + false + + + + /bodyset/pelvis + + 0.0018502799999999999 0.0018455800000000001 0.12253332 + + false + + + + /bodyset/pelvis + + 0.0032796600000000002 0.0052279600000000002 -0.11687518 + + false + + + + /bodyset/pelvis + + -0.165739 0.02683781 0.045750890000000002 + + false + + + + /bodyset/pelvis + + -0.16594252000000001 0.029435739999999998 -0.05179359 + + false + + + + /bodyset/femur_l + + 0.00369258 -0.0019289000000000001 0.00085543999999999995 + + false + + + + /bodyset/femur_r + + 0.0028005 0.00194253 7.8739999999999995e-05 + + false + + + + /bodyset/femur_r + + -0.0084207299999999995 -0.10482345999999999 0.096913669999999993 + + false + + + + /bodyset/femur_r + + 0.04996138 -0.24497677000000001 0.079681650000000007 + + false + + + + /bodyset/femur_r + + -0.017678880000000001 -0.33634233000000002 0.06354245 + + false + + + + /bodyset/femur_r + + 0.0048240399999999999 -0.43925046000000001 0.067789539999999995 + + false + + + + /bodyset/femur_r + + 0.0035122600000000001 -0.44350942999999998 -0.060267979999999999 + + false + + + + /bodyset/tibia_r + + 0.052051970000000003 -0.14016111000000001 0.021610890000000001 + + false + + + + /bodyset/tibia_r + + -0.00421486 -0.19779232999999999 0.078911060000000005 + + false + + + + /bodyset/tibia_r + + 0.041034969999999997 -0.31988657999999998 0.022291999999999999 + + false + + + + /bodyset/tibia_r + + -0.0091964000000000004 -0.43708450999999998 0.048675429999999999 + + false + + + + /bodyset/tibia_r + + 0.01291863 -0.41653917000000001 -0.037232069999999999 + + false + + + + /bodyset/calcn_r + + -0.01376604 0.035681450000000003 -0.020630659999999999 + + false + + + + /bodyset/calcn_r + + 0.19815868 0.056678920000000001 -0.017545640000000001 + + false + + + + /bodyset/calcn_r + + 0.18068443000000001 0.0099199100000000005 0.079395690000000005 + + false + + + + /bodyset/femur_l + + 0.0042950599999999999 -0.11373808000000001 -0.1013666 + + false + + + + /bodyset/femur_l + + 0.069278820000000005 -0.21219747999999999 -0.07746401 + + false + + + + /bodyset/femur_l + + -0.017166600000000001 -0.33238182999999999 -0.070709590000000003 + + false + + + + /bodyset/femur_l + + 0.00362423 -0.43260028 -0.070528820000000006 + + false + + + + /bodyset/femur_l + + 0.0010953499999999999 -0.44377742999999997 0.050770139999999998 + + false + + + + /bodyset/tibia_l + + 0.048703120000000003 -0.13972573999999999 -0.027020519999999999 + + false + + + + /bodyset/tibia_l + + 0.0051297000000000001 -0.23497646999999999 -0.074949959999999996 + + false + + + + /bodyset/tibia_l + + 0.038106180000000003 -0.33266763999999999 -0.01259893 + + false + + + + /bodyset/tibia_l + + -0.01166156 -0.43309235000000001 -0.04404019 + + false + + + + /bodyset/tibia_l + + 0.016140870000000002 -0.41424260000000002 0.039956329999999998 + + false + + + + /bodyset/calcn_l + + -0.01506761 0.045920330000000002 0.020666859999999999 + + false + + + + /bodyset/calcn_l + + 0.19449283000000001 0.059824919999999997 0.025892080000000001 + + false + + + + /bodyset/calcn_l + + 0.17747446 0.01675666 -0.074253929999999996 + + false + + + + + + + + + + /ground + + 0 0 -1.5707963267948966 + + + + true + + 0 1 1 + + + + 2 + + + + + + /bodyset/calcn_r + + 0.0146421 -0.025000000000000001 -0.0122799 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/calcn_r + + 0.153362 -0.01 0.059482899999999998 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/toes_r + + 0.074363000000000026 -0.01 -0.026039 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/calcn_r + + 0.20363700000000001 -0.01 -0.039868800000000003 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/calcn_l + + 0.0146421 -0.025000000000000001 0.0122799 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/calcn_l + + 0.153362 -0.01 -0.059482899999999998 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/toes_l + + 0.074363000000000026 -0.01 0.026039 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + /bodyset/calcn_l + + 0.20363700000000001 -0.01 0.039868800000000003 + + + + true + + 0 1 1 + + + + 2 + + + + 0.035000000000000003 + + + + + + + + + + + diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index 80e4551340..ce4bbaf062 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -580,6 +580,49 @@ MocoTrajectory runForwardSimulation( return forwardSolution; } +// Check that the constraint errors from a MocoSolution are within a specified +// tolerance. Also, check that the slack variables have a reasonable magnitude. +void checkConstraintErrors(const MocoSolution& solution, const Model& model, + bool enforce_constraint_derivatives, const std::string& method) { + StatesTrajectory statesTraj = solution.exportToStatesTrajectory(model); + for (int i = 0; i < (int)statesTraj.getSize(); ++i) { + const auto& s = statesTraj.get(i); + model.realizeAcceleration(s); + const auto& qerr = s.getQErr(); + const auto& uerr = s.getUErr(); + const auto& udoterr = s.getUDotErr(); + + // If we're using the Posa et al. 2016 method and not enforcing + // constraint derivatives, we'll loosen the tolerance for the constraint + // at the midpoint of the mesh intervals, since we do not explicitly + // enforce the constraints at these points. + bool loosen_tol = !enforce_constraint_derivatives && + method == "Posa2016" && + i % 2 != 0; + double tol = loosen_tol ? 1e-3 : 1e-6; + for (int j = 0; j < qerr.size(); ++j) { + REQUIRE_THAT(qerr[j], Catch::Matchers::WithinAbs(0, tol)); + } + if (enforce_constraint_derivatives) { + for (int j = 0; j < uerr.size(); ++j) { + REQUIRE_THAT(uerr[j], Catch::Matchers::WithinAbs(0, tol)); + } + for (int j = 0; j < udoterr.size(); ++j) { + REQUIRE_THAT(udoterr[j], Catch::Matchers::WithinAbs(0, tol)); + } + } + } + + // If problems have converged and all kinematic constraints are satisfied, + // we expect the slack variables to be reasonably small. + const auto& slacks = solution.getSlacksTrajectory(); + for (int icol = 0; icol < slacks.ncol(); ++icol) { + CAPTURE(slacks.col(icol)); + REQUIRE_THAT(SimTK::max(SimTK::abs(slacks.col(icol))), + Catch::Matchers::WithinAbs(0, 1e-5)); + } +} + /// Direct collocation subtests. /// ---------------------------- @@ -589,7 +632,10 @@ MocoTrajectory runForwardSimulation( /// control effort. template void testDoublePendulumPointOnLine( - bool enforce_constraint_derivatives, std::string dynamics_mode) { + bool enforce_constraint_derivatives, std::string dynamics_mode, + const std::string& kinematic_constraint_method = "Posa2016", + const std::string& transcription_scheme = "hermite-simpson", + int num_mesh_intervals = 20) { MocoStudy study; study.setName("double_pendulum_point_on_line"); MocoProblem& mp = study.updProblem(); @@ -625,17 +671,20 @@ void testDoublePendulumPointOnLine( mp.setControlInfo("/tau0", {-100, 100}); mp.setControlInfo("/tau1", {-100, 100}); - mp.addGoal(); + auto* effort = mp.addGoal(); auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(20); + ms.set_num_mesh_intervals(num_mesh_intervals); ms.set_verbosity(2); ms.set_optim_solver("ipopt"); ms.set_optim_convergence_tolerance(1e-3); - ms.set_transcription_scheme("hermite-simpson"); + ms.set_transcription_scheme(transcription_scheme); ms.set_enforce_constraint_derivatives(enforce_constraint_derivatives); - ms.set_minimize_lagrange_multipliers(true); - ms.set_lagrange_multiplier_weight(10); + ms.set_kinematic_constraint_method(kinematic_constraint_method); + if (!enforce_constraint_derivatives) { + ms.set_minimize_lagrange_multipliers(true); + ms.set_lagrange_multiplier_weight(10); + } ms.set_multibody_dynamics_mode(dynamics_mode); ms.setGuess("bounds"); @@ -643,6 +692,13 @@ void testDoublePendulumPointOnLine( solution.write("testConstraints_testDoublePendulumPointOnLine.sto"); // moco.visualize(solution); + // Check that the constraint errors are within a specified tolerance. + model->initSystem(); + checkConstraintErrors(solution, *model, enforce_constraint_derivatives, + kinematic_constraint_method); + + // Check that the end-effector point lies on the vertical line through the + // origin. model->initSystem(); StatesTrajectory states = solution.exportToStatesTrajectory(mp); for (int i = 0; i < (int)states.getSize(); ++i) { @@ -666,8 +722,11 @@ void testDoublePendulumPointOnLine( /// its two coordinates together via a linear relationship and minimizing /// control effort. template -void testDoublePendulumCoordinateCoupler(MocoSolution& solution, - bool enforce_constraint_derivatives, std::string dynamics_mode) { +void testDoublePendulumCoordinateCoupler( + bool enforce_constraint_derivatives, std::string dynamics_mode, + std::string kinematic_constraint_method = "Posa2016", + std::string transcription_scheme = "hermite-simpson", + int num_mesh_intervals = 20) { MocoStudy study; study.setName("double_pendulum_coordinate_coupler"); @@ -705,25 +764,33 @@ void testDoublePendulumCoordinateCoupler(MocoSolution& solution, mp.setStateInfo("/jointset/j1/q1/speed", {-5, 5}, 0, 0); mp.setControlInfo("/tau0", {-50, 50}); mp.setControlInfo("/tau1", {-50, 50}); - mp.addGoal(); + auto* effort = mp.addGoal(); auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(20); + ms.set_num_mesh_intervals(num_mesh_intervals); ms.set_verbosity(2); ms.set_optim_solver("ipopt"); ms.set_optim_convergence_tolerance(1e-3); - ms.set_transcription_scheme("hermite-simpson"); + ms.set_transcription_scheme(transcription_scheme); ms.set_enforce_constraint_derivatives(enforce_constraint_derivatives); - ms.set_minimize_lagrange_multipliers(true); - ms.set_lagrange_multiplier_weight(10); + ms.set_kinematic_constraint_method(kinematic_constraint_method); + if (!enforce_constraint_derivatives) { + ms.set_minimize_lagrange_multipliers(true); + ms.set_lagrange_multiplier_weight(10); + } ms.set_multibody_dynamics_mode(dynamics_mode); ms.setGuess("bounds"); - solution = study.solve(); + MocoSolution solution = study.solve(); solution.write("testConstraints_testDoublePendulumCoordinateCoupler.sto"); - // moco.visualize(solution); + //study.visualize(solution); + // Check that the constraint errors are within a specified tolerance. model->initSystem(); + checkConstraintErrors(solution, *model, enforce_constraint_derivatives, + kinematic_constraint_method); + + // Check that the coordinates are coupled according to the linear function. StatesTrajectory states = solution.exportToStatesTrajectory(mp); for (int i = 0; i < (int)states.getSize(); ++i) { const auto& s = states.get(i); @@ -741,40 +808,36 @@ void testDoublePendulumCoordinateCoupler(MocoSolution& solution, } /// Solve an optimal control problem where a double pendulum must follow a -/// prescribed motion based on the previous test case (see -/// testDoublePendulumCoordinateCoupler). +/// prescribed motion based on sinusoidal functions for each coordinate. template -void testDoublePendulumPrescribedMotion(MocoSolution& couplerSolution, - bool enforce_constraint_derivatives, std::string dynamics_mode) { +void testDoublePendulumPrescribedMotion( + bool enforce_constraint_derivatives, + std::string dynamics_mode, + std::string kinematic_constraint_method = "Posa2016", + std::string transcription_scheme = "hermite-simpson", + int num_mesh_intervals = 20) { MocoStudy study; study.setName("double_pendulum_prescribed_motion"); MocoProblem& mp = study.updProblem(); // Create double pendulum model. auto model = createDoublePendulumModel(); - // Create a spline set for the model states from the previous solution. We - // need to call initSystem() and set the model here in order to convert the - // solution from the previous problem to a StatesTrajectory. model->initSystem(); - mp.setModelAsCopy(*model); - - TimeSeriesTable statesTrajCoupler = - couplerSolution.exportToStatesTrajectory(mp).exportToTable(*model); - GCVSplineSet statesSpline(statesTrajCoupler); // Apply the prescribed motion constraints. + Sine q0func(2.0, 1.0, 0.0); Coordinate& q0 = model->updJointSet().get("j0").updCoordinate(); - q0.setPrescribedFunction(statesSpline.get("/jointset/j0/q0/value")); + q0.setPrescribedFunction(q0func); q0.setDefaultIsPrescribed(true); + Sine q1func(0.5, 2.0, 0.5*SimTK::Pi); Coordinate& q1 = model->updJointSet().get("j1").updCoordinate(); - q1.setPrescribedFunction(statesSpline.get("/jointset/j1/q1/value")); + q1.setPrescribedFunction(q1func); q1.setDefaultIsPrescribed(true); - // Set the model again after implementing the constraints. + + // Set the model after implementing the constraints. mp.setModelAsCopy(*model); mp.setTimeBounds(0, 1); - // No bounds here, since the problem is already highly constrained by the - // prescribed motion constraints on the coordinates. mp.setStateInfo("/jointset/j0/q0/value", {-10, 10}); mp.setStateInfo("/jointset/j0/q0/speed", {-50, 50}); mp.setStateInfo("/jointset/j1/q1/value", {-10, 10}); @@ -782,106 +845,30 @@ void testDoublePendulumPrescribedMotion(MocoSolution& couplerSolution, mp.setControlInfo("/tau0", {-25, 25}); mp.setControlInfo("/tau1", {-25, 25}); - mp.addGoal(); + auto* effort = mp.addGoal(); auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(20); + ms.set_num_mesh_intervals(num_mesh_intervals); ms.set_verbosity(2); ms.set_optim_solver("ipopt"); ms.set_optim_convergence_tolerance(1e-3); - ms.set_transcription_scheme("hermite-simpson"); + ms.set_transcription_scheme(transcription_scheme); ms.set_enforce_constraint_derivatives(enforce_constraint_derivatives); - ms.set_minimize_lagrange_multipliers(true); - ms.set_lagrange_multiplier_weight(10); + ms.set_kinematic_constraint_method(kinematic_constraint_method); + if (!enforce_constraint_derivatives) { + ms.set_minimize_lagrange_multipliers(true); + ms.set_lagrange_multiplier_weight(10); + } ms.set_multibody_dynamics_mode(dynamics_mode); - // Set guess based on coupler solution trajectory. - MocoTrajectory guess(ms.createGuess("bounds")); - guess.setStatesTrajectory(statesTrajCoupler); - ms.setGuess(guess); - - MocoSolution solution = study.solve(); + MocoSolution solution = study.solve().unseal(); solution.write("testConstraints_testDoublePendulumPrescribedMotion.sto"); - // study.visualize(solution); - - // Create a TimeSeriesTable containing the splined state data from - // testDoublePendulumCoordinateCoupler. Since this splined data could be - // somewhat different from the coordinate coupler OCP solution, we use this - // to create a direct comparison between the prescribed motion OCP solution - // states and exactly what the PrescribedMotion constraints should be - // enforcing. - auto statesTraj = solution.exportToStatesTrajectory(mp); - // Initialize data structures to use in the TimeSeriesTable - // convenience constructor. - std::vector indVec((int)statesTraj.getSize()); - SimTK::Matrix depData( - (int)statesTraj.getSize(), (int)solution.getStateNames().size()); - Vector timeVec(1); - for (int i = 0; i < (int)statesTraj.getSize(); ++i) { - const auto& s = statesTraj.get(i); - const SimTK::Real& time = s.getTime(); - indVec[i] = time; - timeVec.updElt(0, 0) = time; - depData.set(i, 0, - statesSpline.get("/jointset/j0/q0/value").calcValue(timeVec)); - depData.set(i, 1, - statesSpline.get("/jointset/j1/q1/value").calcValue(timeVec)); - // The values for the speed states are created from the spline - // derivative values. - depData.set(i, 2, - statesSpline.get("/jointset/j0/q0/value") - .calcDerivative({0}, timeVec)); - depData.set(i, 3, - statesSpline.get("/jointset/j1/q1/value") - .calcDerivative({0}, timeVec)); - } - TimeSeriesTable splineStateValues( - indVec, depData, solution.getStateNames()); - - // Create a MocoTrajectory containing the splined state values. The splined - // state values are also set for the controls and adjuncts as dummy data. - const auto& statesTimes = splineStateValues.getIndependentColumn(); - SimTK::Vector time((int)statesTimes.size(), statesTimes.data(), true); - auto mocoIterSpline = MocoTrajectory(time, - splineStateValues.getColumnLabels(), - splineStateValues.getColumnLabels(), - splineStateValues.getColumnLabels(), {}, - splineStateValues.getMatrix(), splineStateValues.getMatrix(), - splineStateValues.getMatrix(), SimTK::RowVector(0)); - - // Only compare the position-level values between the current solution - // states and the states from the previous test (original and splined). - // These should match well, since position-level values are enforced - // directly via a path constraint in the current problem formulation (see - // MocoTropterSolver for details). - - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(mocoIterSpline, - {{"states", {"/jointset/j0/q0/value", - "/jointset/j1/q1/value"}}}), - 0, 1e-3); - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(couplerSolution, - {{"states", {"/jointset/j0/q0/value", - "/jointset/j1/q1/value"}}}), - 0, 1e-3); - // Only compare the velocity-level values between the current solution - // states and the states from the previous test (original and splined). - // These won't match as well as the position-level values, since velocity- - // level errors are not enforced in the current problem formulation. - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(mocoIterSpline, - {{"states", {"/jointset/j0/q0/speed", - "/jointset/j1/q1/speed"}}}), - 0, 1e-1); - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(couplerSolution, - {{"states", {"/jointset/j0/q0/speed", - "/jointset/j1/q1/speed"}}}), - 0, 1e-1); - // Compare only the actuator controls. These match worse compared to the - // velocity-level states. It is currently unclear to what extent this is - // related to velocity-level states not matching well or the how the model - // constraints are enforced in the current formulation. - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(couplerSolution, - {{"controls", {"/tau0", "/tau1"}}}), - 0, 5); + //study.visualize(solution); + + // Check that the constraint errors are within a specified tolerance. + model->initSystem(); + checkConstraintErrors(solution, *model, enforce_constraint_derivatives, + kinematic_constraint_method); // Run a forward simulation using the solution controls in prescribed // controllers for the model actuators and see if we get the correct states @@ -889,64 +876,127 @@ void testDoublePendulumPrescribedMotion(MocoSolution& couplerSolution, runForwardSimulation(*model, solution, 1e-1); } -TEMPLATE_TEST_CASE("DoublePendulum with and without constraint derivatives", - "[explicit]", MocoCasADiSolver, MocoTropterSolver) { - SECTION("DoublePendulum without constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, false, "explicit"); - testDoublePendulumPrescribedMotion( - couplerSol, false, "explicit"); - } - - SECTION("DoublePendulum with constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, true, "explicit"); - testDoublePendulumPrescribedMotion( - couplerSol, true, "explicit"); +TEST_CASE("DoublePendulum tests, Posa2016 method - MocoCasADiSolver", + "[casadi]") { + bool enforce_constraint_derivatives = GENERATE(true, false); + std::string dynamics_mode = GENERATE(as{}, + "explicit", "implicit"); + std::string section = fmt::format( + "enforce derivatives: {}, dynamics_mode: {}", + enforce_constraint_derivatives, dynamics_mode); + + DYNAMIC_SECTION(section) { + SECTION("CoordinateCouplerConstraint") { + testDoublePendulumCoordinateCoupler( + enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + } + SECTION("PrescribedMotion") { + testDoublePendulumPrescribedMotion( + enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + } + SECTION("PointOnLine") { + testDoublePendulumPointOnLine( + enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + } } } -TEST_CASE("DoublePendulum with and without constraint derivatives", - "[implicit][casadi]") { - SECTION("DoublePendulum without constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, false, "implicit"); - testDoublePendulumPrescribedMotion( - couplerSol, false, "implicit"); - } +TEST_CASE("DoublePendulum tests, Posa2016 method - MocoTropterSolver", + "[tropter]") { + bool enforce_constraint_derivatives = GENERATE(true, false); + std::string section = fmt::format( + "enforce derivatives: {}", enforce_constraint_derivatives); - SECTION("DoublePendulum with constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, true, "implicit"); - testDoublePendulumPrescribedMotion( - couplerSol, true, "implicit"); + DYNAMIC_SECTION(section) { + SECTION("CoordinateCouplerConstraint") { + testDoublePendulumCoordinateCoupler( + enforce_constraint_derivatives, "explicit", "Posa2016"); + } + SECTION("PrescribedMotion") { + testDoublePendulumPrescribedMotion( + enforce_constraint_derivatives, "explicit", "Posa2016"); + } + SECTION("PointOnLine") { + testDoublePendulumPointOnLine( + enforce_constraint_derivatives, "explicit", "Posa2016"); + } } } -TEMPLATE_TEST_CASE("DoublePendulumPointOnLine without constraint derivatives", - "[explicit]", MocoCasADiSolver, MocoTropterSolver) { - testDoublePendulumPointOnLine(false, "explicit"); +TEST_CASE("DoublePendulum tests, Bordalba2023 method", "[casadi]") { + std::string scheme = GENERATE(as{}, + "trapezoidal", "hermite-simpson", "legendre-gauss-3", + "legendre-gauss-radau-3"); + std::string dynamics_mode = GENERATE(as{}, + "explicit", "implicit"); + std::string section = fmt::format("scheme: {}, dynamics_mode: {}", + scheme, dynamics_mode); + + // Trapezoidal rule requires more mesh intervals to keep slack variables + // small. + int num_mesh_intervals = scheme == "trapezoidal" ? 50 : 25; + DYNAMIC_SECTION(section) { + SECTION("CoordinateCouplerConstraint") { + testDoublePendulumCoordinateCoupler(true, + dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals); + } + SECTION("PrescribedMotion") { + testDoublePendulumPrescribedMotion(true, + dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals); + } + SECTION("PointOnLine") { + testDoublePendulumPointOnLine( + true, dynamics_mode, "Bordalba2023", scheme, + num_mesh_intervals); + } + } } -TEMPLATE_TEST_CASE("DoublePendulumPointOnLine with constraint derivatives", - "[explicit]", MocoCasADiSolver, MocoTropterSolver) { - testDoublePendulumPointOnLine(true, "explicit"); -} +TEST_CASE("Bad configurations with kinematic constraints") { + MocoStudy study; + study.setName("double_pendulum_coordinate_coupler"); -TEST_CASE("DoublePendulumPointOnLine without constraint derivatives", - "[implicit][casadi]") { - testDoublePendulumPointOnLine(false, "implicit"); -} + // Create a double pendulum model and add a constraint. + auto model = createDoublePendulumModel(); + const Coordinate& q0 = model->getCoordinateSet().get("q0"); + const Coordinate& q1 = model->getCoordinateSet().get("q1"); + CoordinateCouplerConstraint* constraint = new CoordinateCouplerConstraint(); + Array indepCoordNames; + indepCoordNames.append("q0"); + constraint->setIndependentCoordinateNames(indepCoordNames); + constraint->setDependentCoordinateName("q1"); + const SimTK::Real m = -2; + const SimTK::Real b = SimTK::Pi; + LinearFunction linFunc(m, b); + constraint->setFunction(&linFunc); + model->addConstraint(constraint); + model->finalizeConnections(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + mp.setTimeBounds(0, 1); + + auto& ms = study.initSolver(); -TEST_CASE("DoublePendulumPointOnLine with constraint derivatives", - "[implicit][casadi]") { - testDoublePendulumPointOnLine(true, "implicit"); + SECTION("Enforce constraint derivatives") { + ms.set_enforce_constraint_derivatives(false); + ms.set_kinematic_constraint_method("Bordalba2023"); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring( + "The Bordalba et al. 2023 method for enforcing")); + } + + SECTION("Posa2016 method with Hermite-Simpson only") { + ms.set_enforce_constraint_derivatives(true); + ms.set_kinematic_constraint_method("Posa2016"); + ms.set_transcription_scheme("trapezoidal"); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring( + "Expected the 'hermite-simpson' transcription scheme")); + } } + + class EqualControlConstraint : public MocoPathConstraint { OpenSim_DECLARE_CONCRETE_OBJECT(EqualControlConstraint, MocoPathConstraint); @@ -1068,9 +1118,8 @@ TEMPLATE_TEST_CASE("FailWithPathConstraints", "", // solving for the mass that allows the point mass to obey the constraint // of staying in place. This checks that the parameters are applied to both // ModelBase and ModelDisabledConstraints. -TEMPLATE_TEST_CASE( - "Parameters are set properly for Base and DisabledConstraints", - "[tropter]", MocoTropterSolver /*, too damn slow: MocoCasADiSolver*/) { +TEMPLATE_TEST_CASE("Parameters are set properly", "[tropter]", + MocoTropterSolver /*, too damn slow: MocoCasADiSolver*/) { Model model; auto* body = new Body("b", 0.7, SimTK::Vec3(0), SimTK::Inertia(1)); model.addBody(body); @@ -1179,14 +1228,12 @@ void testDoublePendulumJointReactionGoal(std::string dynamics_mode) { CHECK(solution.getObjective() == Approx(-1. / sqrt(2) * 20).epsilon(1e-2)); } -TEMPLATE_TEST_CASE( - "DoublePendulumPointJointReactionGoal with constraint derivatives", +TEMPLATE_TEST_CASE("Joint reactions w/ constraint derivatives", "[explicit]", MocoCasADiSolver, MocoTropterSolver) { testDoublePendulumJointReactionGoal("explicit"); } -TEMPLATE_TEST_CASE("DoublePendulumJointReactionGoal implicit with " - "constraint derivatives", +TEMPLATE_TEST_CASE("Joint reactions implicit w/ constraint derivatives", "[implicit][casadi]", MocoCasADiSolver) { testDoublePendulumJointReactionGoal("implicit"); } @@ -1395,6 +1442,8 @@ TEST_CASE("Multipliers are correct", "[casadi]") { SECTION("Body welded to ground") { auto dynamics_mode = GENERATE(as{}, "implicit", "explicit"); + auto kinematic_constraint_method = + GENERATE(as{}, "Posa2016", "Bordalba2023"); Model model; const double mass = 1.3169; @@ -1418,6 +1467,7 @@ TEST_CASE("Multipliers are correct", "[casadi]") { auto& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(5); solver.set_multibody_dynamics_mode(dynamics_mode); + solver.set_kinematic_constraint_method(kinematic_constraint_method); solver.set_transcription_scheme("hermite-simpson"); solver.set_enforce_constraint_derivatives(true); @@ -1446,6 +1496,8 @@ TEST_CASE("Multipliers are correct", "[casadi]") { auto dynamics_mode = GENERATE(as{}, "implicit", "explicit"); + auto kinematic_constraint_method = + GENERATE(as{}, "Posa2016", "Bordalba2023"); Model model = ModelFactory::createPlanarPointMass(); model.set_gravity(Vec3(0)); @@ -1476,6 +1528,7 @@ TEST_CASE("Multipliers are correct", "[casadi]") { solver.set_num_mesh_intervals(10); solver.set_multibody_dynamics_mode(dynamics_mode); solver.set_transcription_scheme("hermite-simpson"); + solver.set_kinematic_constraint_method(kinematic_constraint_method); solver.set_enforce_constraint_derivatives(true); MocoSolution solution = study.solve(); const auto Fx = solution.getControl("/forceset/force_x"); @@ -1521,7 +1574,7 @@ TEST_CASE("Prescribed kinematics with kinematic constraints", "[casadi]") { auto& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(10); solver.set_multibody_dynamics_mode("implicit"); - solver.set_interpolate_control_midpoints(false); + solver.set_interpolate_control_mesh_interior_points(false); MocoSolution solution = study.solve(); const auto Fx = solution.getControl("/forceset/force_x"); const auto Fy = solution.getControl("/forceset/force_y"); @@ -2063,3 +2116,104 @@ TEMPLATE_TEST_CASE("Multiple MocoPathConstraints", "", MocoCasADiSolver, study.initSolver(); study.solve(); } + + +class ConstantSpeedConstraint : public Constraint { +OpenSim_DECLARE_CONCRETE_OBJECT(ConstantSpeedConstraint, Constraint); + +public: + OpenSim_DECLARE_SOCKET(body, PhysicalFrame, + "The body participating in this constraint."); + +private: + void extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + const PhysicalFrame& f = getConnectee("body"); + SimTK::MobilizedBody b = f.getMobilizedBody(); + SimTK::Constraint::ConstantSpeed simtkConstantSpeed(b, + SimTK::MobilizerUIndex(0), SimTK::Real(2.34)); + assignConstraintIndex(simtkConstantSpeed.getConstraintIndex()); + } +}; + + +class ConstantAccelerationConstraint : public Constraint { +OpenSim_DECLARE_CONCRETE_OBJECT(ConstantAccelerationConstraint, Constraint); + +public: + OpenSim_DECLARE_SOCKET(body, PhysicalFrame, + "The body participating in this constraint."); + +private: + void extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + const PhysicalFrame& f = getConnectee("body"); + SimTK::MobilizedBody b = f.getMobilizedBody(); + SimTK::Constraint::ConstantAcceleration simtkConstantAcceleration(b, + SimTK::MobilizerUIndex(0), SimTK::Real(1.23)); + assignConstraintIndex(simtkConstantAcceleration.getConstraintIndex()); + } +}; + +MocoStudy createSlidingMassMocoStudy(const Model& model, + const std::string& scheme) { + MocoStudy study; + study.setName("sliding_mass"); + MocoProblem& mp = study.updProblem(); + mp.setModel(make_unique(model)); + mp.setTimeBounds(0, {0, 10}); + mp.setStateInfo("/slider/position/value",{0, 1}, 0, 1); + mp.setStateInfo("/slider/position/speed", {-100, 100}); + mp.addGoal(); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(50); + ms.set_transcription_scheme(scheme); + ms.set_kinematic_constraint_method("Bordalba2023"); + + return study; +} + +TEST_CASE("ConstantSpeedConstraint") { + std::string scheme = GENERATE(as{}, + "trapezoidal", "hermite-simpson", "legendre-gauss-3", + "legendre-gauss-radau-3"); + + Model model = ModelFactory::createSlidingPointMass(); + auto* constraint = new ConstantSpeedConstraint(); + constraint->setName("constant_speed"); + constraint->connectSocket_body(model.getComponent("body")); + model.addConstraint(constraint); + model.initSystem(); + + MocoStudy study = createSlidingMassMocoStudy(model, scheme); + MocoSolution solution = study.solve(); + + const auto& speed = solution.getState("/slider/position/speed"); + for (int itime = 0; itime < solution.getNumTimes(); ++itime) { + CHECK(speed[itime] == Approx(2.34).margin(1e-9)); + } +} + +TEST_CASE("ConstantAccelerationConstraint") { + std::string scheme = GENERATE(as{}, + "trapezoidal", "hermite-simpson", "legendre-gauss-3", + "legendre-gauss-radau-3"); + + Model model = ModelFactory::createSlidingPointMass(); + auto* constraint = new ConstantAccelerationConstraint(); + constraint->setName("constant_acceleration"); + constraint->connectSocket_body(model.getComponent("body")); + model.addConstraint(constraint); + model.initSystem(); + + MocoStudy study = createSlidingMassMocoStudy(model, scheme); + MocoSolution solution = study.solve(); + + const auto& states = solution.exportToStatesTrajectory(model); + for (const auto& state : states) { + model.realizeAcceleration(state); + CHECK(state.getUDot()[0] == Approx(1.23).margin(1e-9)); + } +} + diff --git a/OpenSim/Moco/Test/testMocoContact.cpp b/OpenSim/Moco/Test/testMocoContact.cpp index f694da1601..7e2a5f1d81 100644 --- a/OpenSim/Moco/Test/testMocoContact.cpp +++ b/OpenSim/Moco/Test/testMocoContact.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include "Testing.h" @@ -616,3 +617,101 @@ TEST_CASE("MocoContactTrackingGoal", "[casadi]") { externalLoadsTimeStepping, "ground_force_r_vy", 0.5); } + +// This is a round-trip test. First, use createExternalLoadsTableForGait() to +// create a table of external loads based on a simulation with foot-ground +// contact force elements. Then, use the external loads to apply forces to the +// model with the contact force elements remove and ensure the accelerations +// match the accelerations of the original model. +TEST_CASE("createExternalLoadsTableForGait") { + + // The original model with foot-ground contact elements. + Model model("subject_20dof18musc_running.osim"); + model.initSystem(); + + // A copy of the model with the foot-ground contact elements removed. + Model modelNoContact(model); + modelNoContact.initSystem(); + modelNoContact.updForceSet().clearAndDestroy(); + modelNoContact.updContactGeometrySet().clearAndDestroy(); + + // Load the trajectory. Remove all columns not associated with the skeletal + // kinematics. + TimeSeriesTable trajectory("running_solution_full_stride.sto"); + auto labels = trajectory.getColumnLabels(); + for (const auto& label : labels) { + if (label.find("/jointset") == std::string::npos) { + trajectory.removeColumn(label); + } + } + auto statesTraj = StatesTrajectory::createFromStatesTable(model, trajectory); + + // Create external loads for the calcaneus bodies and apply them to the + // model without contact forces. + std::vector contact_r = {"/forceset/contactHeel_r", + "/forceset/contactLateralMidfoot_r", + "/forceset/contactMedialMidfoot_r"}; + std::vector contact_l = {"/forceset/contactHeel_l", + "/forceset/contactLateralMidfoot_l", + "/forceset/contactMedialMidfoot_l"}; + auto externalLoadsTableCalcn = createExternalLoadsTableForGait(model, + statesTraj, contact_r, contact_l); + + // TODO: avoid writing to file for this conversion. + STOFileAdapter::write(externalLoadsTableCalcn, "external_loads_temp.sto"); + Storage externalLoadsCalcn("external_loads_temp.sto"); + + ExternalForce* externalForceLeftCalcn = new ExternalForce( + externalLoadsCalcn, "ground_force_l_v", "ground_force_l_p", + "ground_torque_l_", "calcn_l"); + modelNoContact.addForce(externalForceLeftCalcn); + + ExternalForce* externalForceRightCalcn = new ExternalForce( + externalLoadsCalcn, "ground_force_r_v", "ground_force_r_p", + "ground_torque_r_", "calcn_r"); + modelNoContact.addForce(externalForceRightCalcn); + modelNoContact.finalizeConnections(); + + // Create external loads for the toes bodies and apply them to the model + // without contact forces. + auto externalLoadsTableToes = createExternalLoadsTableForGait(model, + statesTraj, {"/forceset/contactMedialToe_r"}, + {"/forceset/contactMedialToe_l"}); + + // TODO: avoid writing to file for this conversion. + STOFileAdapter::write(externalLoadsTableToes, "external_loads_temp.sto"); + Storage externalLoadsToes("external_loads_temp.sto"); + + ExternalForce* externalForceLeftToes = new ExternalForce(externalLoadsToes, + "ground_force_l_v", "ground_force_l_p", "ground_torque_l_", + "toes_l"); + modelNoContact.addForce(externalForceLeftToes); + + ExternalForce* externalForceRightToes = new ExternalForce(externalLoadsToes, + "ground_force_r_v", "ground_force_r_p", "ground_torque_r_", + "toes_r"); + modelNoContact.addForce(externalForceRightToes); + modelNoContact.finalizeConnections(); + + // If createExternalLoadsTableForGait() is working correctly, the + // accelerations of `modelNoContact` (i.e., with the external loads applied) + // should match the accelerations of `model` (i.e., the model with the + // foot-ground contact elements). + SimTK::State stateNoContact = modelNoContact.initSystem(); + for (int i = 0; i < static_cast(statesTraj.getSize()); ++i) { + auto state = statesTraj[i]; + model.realizeAcceleration(state); + + // Set the kinematic state of the model without contact forces to match + // the original model's state. + stateNoContact.setTime(state.getTime()); + stateNoContact.setQ(state.getQ()); + stateNoContact.setU(state.getU()); + modelNoContact.realizeAcceleration(stateNoContact); + + // Compare accelerations. + SimTK::Vector error = state.getUDot() - stateNoContact.getUDot(); + CAPTURE(error); + CHECK_THAT(error.norm(), Catch::Matchers::WithinAbs(0, 1e-8)); + } +} \ No newline at end of file diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index 1258846d87..bbcda978ca 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -19,13 +19,15 @@ #include #include #include -#include +#include +#include #include +#include #include #include -#include #include "Testing.h" +#include using Catch::Approx; using Catch::Matchers::ContainsSubstring; @@ -290,7 +292,7 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, MocoStudy study = setupMocoStudyDoublePendulumMinimizeEffort(); auto solutionEffort = study.solve(); - SECTION ("MocoControlTrackingGoal") { + SECTION("MocoControlTrackingGoal") { // Re-run problem, now setting effort cost function to zero and adding a // control tracking cost. auto& problem = study.updProblem(); @@ -322,7 +324,7 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, solutionTracking.getStatesTrajectory(), 1e-2); } - SECTION ("MocoOrientationTrackingGoal") { + SECTION("MocoOrientationTrackingGoal") { MocoStudy studyOrientationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); @@ -343,21 +345,21 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, } } - SECTION ("MocoTranslationTrackingGoal") { + SECTION("MocoTranslationTrackingGoal") { MocoStudy studyTranslationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); testDoublePendulumTracking( studyTranslationTracking, solutionEffort); } - SECTION ("MocoAngularVelocityTrackingGoal") { + SECTION("MocoAngularVelocityTrackingGoal") { MocoStudy studyAngularVelocityTracking = setupMocoStudyDoublePendulumMinimizeEffort(); testDoublePendulumTracking( studyAngularVelocityTracking, solutionEffort); } - SECTION ("MocoAccelerationTrackingGoal") { + SECTION("MocoAccelerationTrackingGoal") { MocoStudy studyAccelerationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); // Re-run problem, now setting effort cost function to a low weight and @@ -390,7 +392,7 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, solutionTracking.getStatesTrajectory(), 1e-2); } - SECTION ("MocoAccelerationTrackingGoal (IMU tracking)") { + SECTION("MocoAccelerationTrackingGoal (IMU tracking)") { MocoStudy studyAccelerationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); // Re-run problem, now setting effort cost function to a low weight and @@ -436,14 +438,42 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, auto* genForceTracking = problem.addGoal("tracking"); genForceTracking->setReference(generalizedForces); - studyGenForceTracking.updSolver().resetProblem(problem); - auto solutionTracking = studyGenForceTracking.solve(); - // The tracking solution should match the effort solution. - SimTK_TEST_EQ_TOL(solutionEffort.getControlsTrajectory(), - solutionTracking.getControlsTrajectory(), 1e-3); - SimTK_TEST_EQ_TOL(solutionEffort.getStatesTrajectory(), - solutionTracking.getStatesTrajectory(), 1e-3); + SECTION("Setting weights") { + Model model = problem.createRep().getModelBase(); + SimTK::State state = model.initSystem(); + // Set acclelerations to zero, so the only model-generated forces + // in the integrand are zero. + state.updUDot() = SimTK::Vector(state.getNU(), 0.0); + MocoGoal::IntegrandInput input{0, state, {}}; + + // Apply weights. + genForceTracking->setWeightForGeneralizedForcePattern("q.*", 5.0); + genForceTracking->setWeightForGeneralizedForce("q1_moment", 10.0); + genForceTracking->initializeOnModel(model); + + // The integrand should be the sum of the squared generalized forces + // multiplied by the weights. + const SimTK::RowVectorView initialGenForces = + generalizedForces.getRowAtIndex(0); + double integrand = + 5.0 * (initialGenForces[0] * initialGenForces[0]) + + 10.0 * (initialGenForces[1] * initialGenForces[1]); + CHECK_THAT(genForceTracking->calcIntegrand(input), + Catch::Matchers::WithinAbs(integrand, 1e-6)); + } + + SECTION("Tracking performance") { + studyGenForceTracking.updSolver().resetProblem(problem); + auto solutionTracking = studyGenForceTracking.solve(); + + // The tracking solution should match the effort solution. + SimTK_TEST_EQ_TOL(solutionEffort.getControlsTrajectory(), + solutionTracking.getControlsTrajectory(), 1e-3); + SimTK_TEST_EQ_TOL(solutionEffort.getStatesTrajectory(), + solutionTracking.getStatesTrajectory(), 1e-3); + } + } } @@ -619,8 +649,7 @@ TEST_CASE("Test MocoSumSquaredStateGoal") { std::string q1_str = q1.getAbsolutePathString() + "/value"; SimTK::State state = model.initSystem(); - SimTK::Vector inputControls; - MocoGoal::IntegrandInput input {0, state, {}}; + MocoGoal::IntegrandInput input{0, state, {}}; q0.setValue(state, 1.0); q1.setValue(state, 0.5); @@ -1413,3 +1442,203 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { "testMocoGoals_MocoFrameDistanceConstraint_study.omoco"); } } + +TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { + SECTION("mass goal") { + MocoStudy study; + Model model = ModelFactory::createSlidingPointMass(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", + MocoBounds(0, 10)); + auto* mass_goal = mp.addGoal(); + mass_goal->setExpression("(q-4)^2"); + mass_goal->addParameter(*parameter, "q"); + auto* effort_goal = mp.addGoal(); + effort_goal->setWeight(0.001); + effort_goal->setName("effort"); + + auto& ms = study.initTropterSolver(); + ms.set_num_mesh_intervals(25); + MocoSolution sol = study.solve(); + + // 3.998 + CHECK(sol.getParameter("sphere_mass") == Catch::Approx(4).epsilon(1e-2)); + } + + SECTION("two parameter mass goal") { + MocoStudy study; + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); + mp.setStateInfo("/slider/position/speed", {-20, 20}); + mp.setStateInfo("/slider2/position/value", {-5, 5}, 1, {1.2, 1.3}); + mp.setStateInfo("/slider2/position/speed", {-20, 20}); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", + MocoBounds(0, 10)); + auto* parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", + MocoBounds(0, 10)); + int total_weight = 7; + auto* mass_goal = mp.addGoal(); + mass_goal->setExpression(fmt::format("(p+q-{})^2", total_weight)); + mass_goal->addParameter(*parameter, "p"); + mass_goal->addParameter(*parameter2, "q"); + + auto& ms = study.initTropterSolver(); + ms.set_num_mesh_intervals(25); + MocoSolution sol = study.solve(); + + // 3.7 and 3.3 + CHECK(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass") + == Catch::Approx(total_weight).epsilon(1e-9)); + } +} + +// from testMocoParameters +const double STIFFNESS = 100.0; // N/m +const double MASS = 5.0; // kg +const double FINAL_TIME = SimTK::Pi * sqrt(MASS / STIFFNESS); +std::unique_ptr createOscillatorTwoSpringsModel() { + auto model = make_unique(); + model->setName("oscillator_two_springs"); + model->set_gravity(SimTK::Vec3(0, 0, 0)); + auto* body = new Body("body", MASS, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("slider", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* spring1 = new SpringGeneralizedForce(); + spring1->setName("spring1"); + spring1->set_coordinate("position"); + spring1->setRestLength(0.0); + spring1->setStiffness(0.25*STIFFNESS); + spring1->setViscosity(0.0); + model->addComponent(spring1); + + auto* spring2 = new SpringGeneralizedForce(); + spring2->setName("spring2"); + spring2->set_coordinate("position"); + spring2->setRestLength(0.0); + spring2->setStiffness(0.25*STIFFNESS); + spring2->setViscosity(0.0); + model->addComponent(spring2); + + return model; +} + +TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { + MocoStudy study; + study.setName("oscillator_spring_stiffnesses"); + MocoProblem& mp = study.updProblem(); + mp.setModel(createOscillatorTwoSpringsModel()); + mp.setTimeBounds(0, FINAL_TIME); + mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + SECTION("single parameter for two values") { + // create a parameter goal for the stiffness of both strings + std::vector components = {"spring1", "spring2"}; + auto* parameter = mp.addParameter("spring_stiffness", components, + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + // minimum is when p = 0.5*STIFFNESS + spring_goal->setExpression(fmt::format("(p-{})^2", 0.5*STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") == + Catch::Approx(0.5*STIFFNESS).epsilon(1e-6)); + } + + SECTION("two parameters") { + // create two parameters to include in the goal + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + // minimum is when p + q = STIFFNESS + spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + spring_goal->addParameter(*parameter2, "q"); + + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") + + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } + + SECTION("missing parameter") { + // create one parameter but have two variables + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal( + "stiffness", 1, fmt::format("(p+q-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + + CHECK_THROWS(study.initCasADiSolver()); // missing q + } + + SECTION("extra parameter") { + // create two parameters for the goal, only one is used + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal( + "stiffness", 1, fmt::format("(p-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + // second parameter is ignored + spring_goal->addParameter(*parameter2, "a"); + + CHECK_NOTHROW(study.initCasADiSolver()); + } + + SECTION("endpoint goal") { + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + spring_goal->addParameter(*parameter2, "q"); + // set as endpoint constraint + spring_goal->setMode("endpoint_constraint"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") + + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } +} diff --git a/OpenSim/Moco/Test/testMocoInverse.cpp b/OpenSim/Moco/Test/testMocoInverse.cpp index 7c0c867fcf..375d674dd6 100644 --- a/OpenSim/Moco/Test/testMocoInverse.cpp +++ b/OpenSim/Moco/Test/testMocoInverse.cpp @@ -194,7 +194,7 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles", "[casadi]") { auto& solver = study.updSolver(); solver.resetProblem(problem); - solver.set_enforce_path_constraint_midpoints(true); + solver.set_enforce_path_constraint_mesh_interior_points(true); MocoSolution solution = study.solve(); @@ -207,7 +207,19 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles", "[casadi]") { CHECK(med_gas_l_excitation[i] <= Approx(0.1).margin(1e-6)); } + } + SECTION("initializeKinematics") { + auto kinematics = inverse.initializeKinematics(); + // check that some of the expected columns are there + CHECK_NOTHROW(kinematics.getColumnIndex( + "/jointset/hip_r/hip_rotation_r/value")); + CHECK_NOTHROW(kinematics.getColumnIndex( + "/jointset/ground_pelvis/pelvis_tilt/value")); + CHECK_NOTHROW(kinematics.getColumnIndex( + "/jointset/walker_knee_r/knee_angle_r/speed")); + CHECK_NOTHROW(kinematics.getColumnIndex( + "/jointset/back/lumbar_extension/speed")); } } // Next test_case fails on linux while parsing .sto file, disabling for now diff --git a/OpenSim/Moco/osimMoco.h b/OpenSim/Moco/osimMoco.h index c94688ec6d..cc39e2c268 100644 --- a/OpenSim/Moco/osimMoco.h +++ b/OpenSim/Moco/osimMoco.h @@ -35,6 +35,7 @@ #include "MocoGoal/MocoContactTrackingGoal.h" #include "MocoGoal/MocoControlGoal.h" #include "MocoGoal/MocoControlTrackingGoal.h" +#include "MocoGoal/MocoExpressionBasedParameterGoal.h" #include "MocoGoal/MocoInitialActivationGoal.h" #include "MocoGoal/MocoInitialForceEquilibriumDGFGoal.h" #include "MocoGoal/MocoInitialVelocityEquilibriumDGFGoal.h" diff --git a/OpenSim/Moco/tropter/TropterProblem.cpp b/OpenSim/Moco/tropter/TropterProblem.cpp index 3e45dd3f0f..19aeae7bc7 100644 --- a/OpenSim/Moco/tropter/TropterProblem.cpp +++ b/OpenSim/Moco/tropter/TropterProblem.cpp @@ -143,9 +143,11 @@ convertIterateTropterToMoco(const tropIterateType& tropSol, input_control_names, multiplier_names, derivative_names, parameter_names, states, controls, input_controls, multipliers, derivatives, parameters); - // Append slack variables. + + // Append slack variables. Interpolate slack variables to remove NaNs. for (int i = 0; i < numSlacks; ++i) { - mocoIter.appendSlack(slack_names[i], slacks.col(i)); + mocoIter.appendSlack(slack_names[i], + interpolate(time, slacks.col(i), time, true, true)); } return mocoIter; } diff --git a/OpenSim/Sandbox/ImuStreaming.cpp b/OpenSim/Sandbox/ImuStreaming.cpp index 210ccc9cb2..21e8ad19f4 100644 --- a/OpenSim/Sandbox/ImuStreaming.cpp +++ b/OpenSim/Sandbox/ImuStreaming.cpp @@ -142,22 +142,22 @@ parseImuData(const char* str, const size_t len) { std::smatch result_ts{}; if(!std::regex_search(data, result_ts, regex_ts)) throw std::runtime_error{"No timestamp in data stream."}; - double timestamp{std::stod(result_ts[0])}; + double timestamp{OpenSim::IO::stod(result_ts[0])}; // Acceleration. std::array gravity{}; std::smatch result_gravity{}; if(std::regex_search(data, result_gravity, regex_gravity)) { - gravity[0] = std::stod(result_gravity[1]); - gravity[1] = std::stod(result_gravity[2]); - gravity[2] = std::stod(result_gravity[3]); + gravity[0] = OpenSim::IO::stod(result_gravity[1]); + gravity[1] = OpenSim::IO::stod(result_gravity[2]); + gravity[2] = OpenSim::IO::stod(result_gravity[3]); } // Angular veclocity. std::array omega{}; std::smatch result_omega{}; if(std::regex_search(data, result_omega, regex_omega)) { - omega[0] = std::stod(result_omega[1]); - omega[1] = std::stod(result_omega[2]); - omega[2] = std::stod(result_omega[3]); + omega[0] = OpenSim::IO::stod(result_omega[1]); + omega[1] = OpenSim::IO::stod(result_omega[2]); + omega[2] = OpenSim::IO::stod(result_omega[3]); } return std::make_tuple(timestamp, gravity, omega); diff --git a/OpenSim/Sandbox/Moco/sandboxContact.cpp b/OpenSim/Sandbox/Moco/sandboxContact.cpp index d8d4240e60..909db43fda 100644 --- a/OpenSim/Sandbox/Moco/sandboxContact.cpp +++ b/OpenSim/Sandbox/Moco/sandboxContact.cpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include //#include #include @@ -42,13 +44,15 @@ using namespace OpenSim; using SimTK::Vec3; -class CustomContactForce : public Force { - OpenSim_DECLARE_CONCRETE_OBJECT(CustomContactForce, Force); +class CustomContactForce : public ForceProducer { + OpenSim_DECLARE_CONCRETE_OBJECT(CustomContactForce, ForceProducer); public: OpenSim_DECLARE_SOCKET(station, Station, "TODO"); - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& /*generalizedForces*/) const override { + +private: + void implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const override { + const auto& pt = getConnectee("station"); const auto& pos = pt.getLocationInGround(s); const auto& vel = pt.getVelocityInGround(s); @@ -65,10 +69,9 @@ class CustomContactForce : public Force { } const SimTK::Real voidStiffness = 1.0; // N/m force[1] += voidStiffness * depth; - //applyGeneralizedForce(s, getModel().getCoordinateSet().get(0), - // force, generalizedForces); - applyForceToPoint(s, pt.getParentFrame(), pt.get_location(), force, - bodyForces); + // forceConsumer.consumeGeneralizedForce(s, getModel().getCoordinateSet().get(0), force); + + forceConsumer.consumePointForce(s, pt.getParentFrame(), pt.get_location(), force); // TODO equal and opposite force on ground. } }; diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 8e30852df9..b17448daa7 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -24,6 +24,5 @@ using namespace OpenSim; int main() { - return EXIT_SUCCESS; } diff --git a/OpenSim/Sandbox/Moco/sandboxWholeBodyTracking/sandboxWholeBodyTracking.cpp b/OpenSim/Sandbox/Moco/sandboxWholeBodyTracking/sandboxWholeBodyTracking.cpp index 7f58090353..026d19c3ad 100644 --- a/OpenSim/Sandbox/Moco/sandboxWholeBodyTracking/sandboxWholeBodyTracking.cpp +++ b/OpenSim/Sandbox/Moco/sandboxWholeBodyTracking/sandboxWholeBodyTracking.cpp @@ -18,13 +18,15 @@ #include #include +#include +#include #include #include using namespace OpenSim; -class /*TODO OSIMMOCO_API*/AckermannVanDenBogert2010Force : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, Force); +class /*TODO OSIMMOCO_API*/AckermannVanDenBogert2010Force : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, ForceProducer); public: OpenSim_DECLARE_PROPERTY(stiffness, double, "TODO N/m^3"); OpenSim_DECLARE_PROPERTY(dissipation, double, "TODO s/m"); @@ -73,16 +75,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, Force); force[0] = frictionForce; return force; } - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& /*generalizedForces*/) const override { - const SimTK::Vec3 force = calcContactForce(s); - const auto& pt = getConnectee("station"); - const auto& pos = pt.getLocationInGround(s); - const auto& frame = pt.getParentFrame(); - applyForceToPoint(s, frame, pt.get_location(), force, bodyForces); - applyForceToPoint(s, getModel().getGround(), pos, -force, bodyForces); - } OpenSim::Array getRecordLabels() const override { OpenSim::Array labels; @@ -105,6 +97,17 @@ OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force, Force); // TODO potential energy. private: + void implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const override { + + const SimTK::Vec3 force = calcContactForce(s); + const auto& pt = getConnectee("station"); + const auto& pos = pt.getLocationInGround(s); + const auto& frame = pt.getParentFrame(); + forceConsumer.consumePointForce(s, frame, pt.get_location(), force); + forceConsumer.consumePointForce(s, getModel().getGround(), pos, -force); + } + void constructProperties() { constructProperty_friction_coefficient(1.0); constructProperty_stiffness(5e7); diff --git a/OpenSim/Sandbox/xsens/CMakeLists.txt b/OpenSim/Sandbox/xsens/CMakeLists.txt index a1376962a4..8e976ea97e 100644 --- a/OpenSim/Sandbox/xsens/CMakeLists.txt +++ b/OpenSim/Sandbox/xsens/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT WIN32) endif() set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) set(XSENS_SDK_DIR "" CACHE PATH "Directory containing XSENS SDK.") diff --git a/OpenSim/Simulation/MarkersReference.cpp b/OpenSim/Simulation/MarkersReference.cpp index c36326cb52..de5c01b203 100644 --- a/OpenSim/Simulation/MarkersReference.cpp +++ b/OpenSim/Simulation/MarkersReference.cpp @@ -22,6 +22,7 @@ * -------------------------------------------------------------------------- */ #include "MarkersReference.h" +#include "OpenSim/Common/IO.h" #include #include @@ -228,7 +229,7 @@ double MarkersReference::getSamplingFrequency() const { if(_markerTable.hasTableMetaDataKey("DataRate")) { auto datarate = _markerTable.getTableMetaData("DataRate"); - return std::stod(datarate); + return OpenSim::IO::stod(datarate); } else return SimTK::NaN; } diff --git a/OpenSim/Simulation/Model/AbstractGeometryPath.cpp b/OpenSim/Simulation/Model/AbstractGeometryPath.cpp index 637e091b20..9aead5eaed 100644 --- a/OpenSim/Simulation/Model/AbstractGeometryPath.cpp +++ b/OpenSim/Simulation/Model/AbstractGeometryPath.cpp @@ -23,6 +23,9 @@ #include "AbstractGeometryPath.h" +#include +#include + using namespace OpenSim; //============================================================================= @@ -50,6 +53,15 @@ AbstractGeometryPath::AbstractGeometryPath( AbstractGeometryPath& AbstractGeometryPath::operator=( AbstractGeometryPath&& other) = default; +void AbstractGeometryPath::addInEquivalentForces(const SimTK::State& state, + const double& tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const +{ + ForceApplier forceApplier{&getModel().getMatterSubsystem(), &bodyForces, &mobilityForces}; + produceForces(state, tension, forceApplier); +} + //============================================================================= // DEFAULTED METHODS //============================================================================= diff --git a/OpenSim/Simulation/Model/AbstractGeometryPath.h b/OpenSim/Simulation/Model/AbstractGeometryPath.h index 417fbf9df2..7270bb3417 100644 --- a/OpenSim/Simulation/Model/AbstractGeometryPath.h +++ b/OpenSim/Simulation/Model/AbstractGeometryPath.h @@ -34,9 +34,10 @@ #endif #endif -namespace OpenSim { +namespace OpenSim { class Coordinate; } +namespace OpenSim { class ForceConsumer; } -class Coordinate; +namespace OpenSim { //============================================================================= // ABSTRACT GEOMETRY PATH @@ -112,19 +113,34 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(AbstractGeometryPath, ModelComponent); */ virtual double getLengtheningSpeed(const SimTK::State& s) const = 0; + /** + * Requests that the concrete implementation produces forces resulting from + * applying a tension along its path, emitting them into the supplied + * `ForceConsumer`. + * + * @param state the state used to evaluate forces + * @param tension scalar of the applied (+ve) tensile force + * @param forceConsumer a `ForceConsumer` shall receive each produced force + */ + virtual void produceForces(const SimTK::State& state, + double tension, + ForceConsumer& forceConsumer) const = 0; + /** * Add in the equivalent body and generalized forces to be applied to the * multibody system resulting from a tension along the AbstractGeometryPath. * + * Note: this internally uses `produceForces` + * * @param state state used to evaluate forces * @param[in] tension scalar of the applied (+ve) tensile force * @param[in,out] bodyForces Vector of forces (SpatialVec's) on bodies * @param[in,out] mobilityForces Vector of generalized forces */ - virtual void addInEquivalentForces(const SimTK::State& state, + void addInEquivalentForces(const SimTK::State& state, const double& tension, SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const = 0; + SimTK::Vector& mobilityForces) const; /** * Returns the moment arm of the path in the given state with respect to diff --git a/OpenSim/Simulation/Model/Actuator.h b/OpenSim/Simulation/Model/Actuator.h index a1c0be7fc7..b14baf285d 100644 --- a/OpenSim/Simulation/Model/Actuator.h +++ b/OpenSim/Simulation/Model/Actuator.h @@ -24,7 +24,7 @@ * -------------------------------------------------------------------------- */ #include -#include "Force.h" +#include #ifdef SWIG @@ -46,8 +46,8 @@ namespace OpenSim { * * @author Ajay Seth */ -class OSIMSIMULATION_API Actuator : public Force { -OpenSim_DECLARE_ABSTRACT_OBJECT(Actuator, Force); +class OSIMSIMULATION_API Actuator : public ForceProducer { +OpenSim_DECLARE_ABSTRACT_OBJECT(Actuator, ForceProducer); //============================================================================= // NO PROPERTIES //============================================================================= diff --git a/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp b/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp index f62a9dce6c..95424f8ba0 100644 --- a/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp +++ b/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp @@ -30,7 +30,8 @@ using namespace OpenSim; // CONSTRUCTORS //============================================================================= -Blankevoort1991Ligament::Blankevoort1991Ligament() : Force() { +Blankevoort1991Ligament::Blankevoort1991Ligament() +{ constructProperties(); setNull(); } @@ -331,18 +332,9 @@ double Blankevoort1991Ligament::calcTotalForce(const SimTK::State& s) const { return force_total; } -void Blankevoort1991Ligament::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const { - if (get_appliesForce()) { - // total force - double force_total = getTotalForce(s); - - const AbstractGeometryPath&path = getPath(); - - path.addInEquivalentForces( - s, force_total, bodyForces, generalizedForces); - } +void Blankevoort1991Ligament::implProduceForces(const SimTK::State& s, ForceConsumer& forceConsumer) const +{ + getPath().produceForces(s, getTotalForce(s), forceConsumer); } double Blankevoort1991Ligament::computePotentialEnergy( diff --git a/OpenSim/Simulation/Model/Blankevoort1991Ligament.h b/OpenSim/Simulation/Model/Blankevoort1991Ligament.h index 5c7d5c02c8..ec17c29124 100644 --- a/OpenSim/Simulation/Model/Blankevoort1991Ligament.h +++ b/OpenSim/Simulation/Model/Blankevoort1991Ligament.h @@ -24,7 +24,7 @@ * -------------------------------------------------------------------------- */ #include -#include +#include #include namespace OpenSim { @@ -170,8 +170,8 @@ affected by scaling the model. */ -class OSIMSIMULATION_API Blankevoort1991Ligament : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(Blankevoort1991Ligament, Force) +class OSIMSIMULATION_API Blankevoort1991Ligament : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(Blankevoort1991Ligament, ForceProducer) public: //============================================================================= @@ -329,10 +329,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Blankevoort1991Ligament, Force) double computeMomentArm( const SimTK::State& s, Coordinate& aCoord) const; - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - double computePotentialEnergy( const SimTK::State& state) const override; @@ -360,6 +356,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Blankevoort1991Ligament, Force) double calcInverseForceStrainCurve(double force) const; private: + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void setNull(); void constructProperties(); //============================================================================= diff --git a/OpenSim/Simulation/Model/CoordinateLimitForce.cpp b/OpenSim/Simulation/Model/CoordinateLimitForce.cpp index effd14e557..a712e594d2 100644 --- a/OpenSim/Simulation/Model/CoordinateLimitForce.cpp +++ b/OpenSim/Simulation/Model/CoordinateLimitForce.cpp @@ -26,6 +26,8 @@ // INCLUDES //============================================================================= #include "CoordinateLimitForce.h" + +#include #include #include @@ -48,7 +50,7 @@ CoordinateLimitForce::CoordinateLimitForce() CoordinateLimitForce::CoordinateLimitForce (const string& coordName, double q_upper, double K_upper, double q_lower, double K_lower, double damping, double dq, - bool computeDissipationEnergy) : Force() + bool computeDissipationEnergy) { setNull(); constructProperties(); @@ -250,11 +252,10 @@ void CoordinateLimitForce::extendAddToSystem(SimTK::MultibodySystem& system) con * Compute and apply the mobility force corresponding to the passive limit force * */ -void CoordinateLimitForce::computeForce( const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const +void CoordinateLimitForce::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const { - applyGeneralizedForce(s, *_coord, calcLimitForce(s), mobilityForces); + forceConsumer.consumeGeneralizedForce(s, *_coord, calcLimitForce(s)); } double CoordinateLimitForce::calcLimitForce( const SimTK::State& s) const diff --git a/OpenSim/Simulation/Model/CoordinateLimitForce.h b/OpenSim/Simulation/Model/CoordinateLimitForce.h index 1d8574d3de..b752a7589e 100644 --- a/OpenSim/Simulation/Model/CoordinateLimitForce.h +++ b/OpenSim/Simulation/Model/CoordinateLimitForce.h @@ -27,7 +27,7 @@ //============================================================================= // INCLUDES //============================================================================= -#include +#include //============================================================================= @@ -37,10 +37,10 @@ namespace OpenSim { /** * Generate a force that acts to limit the range of motion of a coordinate. * Force is experienced at upper and lower limits of the coordinate value - * according to a constant stiffnesses K_upper and K_lower, with a C2 continuous + * according to constant stiffnesses K_upper and K_lower, with a C2-continuous * transition from 0 to K. The transition parameter defines how far beyond the * limit the stiffness becomes constant. The integrator will like smoother - * (i.e. larger transition regions). + * (i.e. larger) transition regions. * * Damping factor is also phased in through the transition region from 0 to the * value provided. @@ -48,15 +48,15 @@ namespace OpenSim { * Limiting force is guaranteed to be zero within the upper and lower limits. * * The potential energy stored in the spring component of the force is - * accessible as well as the power (nd optionally energy) dissipated. + * accessible as well as the power (and, optionally, energy) dissipated. * The function has the following shape: * * \image html coordinate_limit_force.png * * @author Ajay Seth */ -class OSIMSIMULATION_API CoordinateLimitForce : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(CoordinateLimitForce, Force); +class OSIMSIMULATION_API CoordinateLimitForce : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(CoordinateLimitForce, ForceProducer); public: //============================================================================== // PROPERTIES @@ -217,14 +217,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CoordinateLimitForce, Force); /** Create the underlying Force that is part of the multibody system. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; - //-------------------------------------------------------------------------- - // Force Interface - //-------------------------------------------------------------------------- - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; - private: + /** + * Implements the `ForceProducer` interface. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + // Object helpers void setNull(); void constructProperties(); diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index e876f8fb19..b0f8dac9ba 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -318,13 +318,12 @@ SimTK::Vec6 ExpressionBasedBushingForce::calcBushingForce( /* Compute the force contribution to the system and add in to appropriate * bodyForce and/or system generalizedForce. */ -void ExpressionBasedBushingForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void ExpressionBasedBushingForce::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const { // convert internal forces to spatial and add then add to system // physical (body) forces - addInPhysicalForcesFromInternal(s, calcBushingForce(s), bodyForces); + producePhysicalForcesFromInternal(s, calcBushingForce(s), forceConsumer); } //============================================================================= diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h index 83d75f86c3..47863d9217 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h @@ -25,7 +25,7 @@ // INCLUDE -#include "Force.h" +#include #include namespace OpenSim { @@ -50,7 +50,7 @@ namespace OpenSim { * @author Matt DeMers */ class OSIMSIMULATION_API ExpressionBasedBushingForce - : public TwoFrameLinker { + : public TwoFrameLinker { OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); public: //============================================================================== @@ -251,11 +251,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); virtual OpenSim::Array getRecordValues(const SimTK::State& state) const override; protected: - /** Compute the bushing force contribution to the system and add in to - appropriate bodyForces and/or system generalizedForces */ - virtual void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; /** Potential energy calculation is not implemented */ //-------------------------------------------------------------------------- // Visual support in SimTK visualizer @@ -271,6 +266,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); SimTK::SpatialVec& forces_on_F_in_ground) const; private: + /** + * Implements the `ForceProducer` interface by computing the bushing force. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + //-------------------------------------------------------------------------- // Implement ModelComponent interface. //-------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp index c54dd59fdf..bb76b44179 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp @@ -25,6 +25,8 @@ // INCLUDES //============================================================================= #include "ExpressionBasedCoordinateForce.h" + +#include #include #include #include @@ -120,15 +122,10 @@ void ExpressionBasedCoordinateForce:: this->_forceMagnitudeCV = addCacheVariable("force_magnitude", 0.0, SimTK::Stage::Velocity); } -//============================================================================= -// Computing -//============================================================================= -// Compute and apply the force -void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void ExpressionBasedCoordinateForce::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const { - applyGeneralizedForce(s, *_coord, calcExpressionForce(s), generalizedForces); + forceConsumer.consumeGeneralizedForce(s, *_coord, calcExpressionForce(s)); } // Compute the force diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h index e6e0b3313a..5d594883b2 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h @@ -23,14 +23,14 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ // INCLUDE -#include "Force.h" +#include #include namespace OpenSim { -class OSIMSIMULATION_API ExpressionBasedCoordinateForce : public Force +class OSIMSIMULATION_API ExpressionBasedCoordinateForce : public ForceProducer { -OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); +OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, ForceProducer); public: //============================================================================== // PROPERTIES @@ -90,15 +90,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); const double& getForceMagnitude(const SimTK::State& state) const; -//============================================================================== -// COMPUTATION -//============================================================================== - /** Compute the coordinate force based on the user-defined expression - and apply it to the model */ - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - //-------------------------------------------------------------------------- // COMPUTATIONS //-------------------------------------------------------------------------- @@ -128,6 +119,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); private: + /** + * Implements the `ForceProducer` API by computing the coordinate force based + * on the user-defined expression. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void setNull(); void constructProperties(); diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp index 5560d0d942..c5dbcb23c6 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp @@ -25,7 +25,10 @@ // INCLUDES //============================================================================= #include "ExpressionBasedPointToPointForce.h" + #include +#include + #include #include @@ -153,24 +156,20 @@ extendAddToSystem(SimTK::MultibodySystem& system) const // Beyond the const Component get access to underlying SimTK elements ExpressionBasedPointToPointForce* mutableThis = const_cast(this); - - // Get underlying mobilized bodies - mutableThis->_b1 = _body1->getMobilizedBody(); - mutableThis->_b2 = _body2->getMobilizedBody(); } //============================================================================= // Computing //============================================================================= -// Compute and apply the force -void ExpressionBasedPointToPointForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +// compute and produce forces +void ExpressionBasedPointToPointForce::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { using namespace SimTK; - const Transform& X_GB1 = _b1->getBodyTransform(s); - const Transform& X_GB2 = _b2->getBodyTransform(s); + const Transform& X_GB1 = _body1->getMobilizedBody().getBodyTransform(s); + const Transform& X_GB2 = _body2->getMobilizedBody().getBodyTransform(s); const Vec3 s1_G = X_GB1.R() * getPoint1(); const Vec3 s2_G = X_GB2.R() * getPoint2(); @@ -180,8 +179,8 @@ void ExpressionBasedPointToPointForce::computeForce(const SimTK::State& s, const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2 const double d = r_G.norm(); // distance between the points - const Vec3 v1_G = _b1->findStationVelocityInGround(s, getPoint1()); - const Vec3 v2_G = _b2->findStationVelocityInGround(s, getPoint2()); + const Vec3 v1_G = _body1->getMobilizedBody().findStationVelocityInGround(s, getPoint1()); + const Vec3 v2_G = _body2->getMobilizedBody().findStationVelocityInGround(s, getPoint2()); const Vec3 vRel = v2_G - v1_G; // relative velocity //speed along the line connecting the two bodies @@ -196,8 +195,8 @@ void ExpressionBasedPointToPointForce::computeForce(const SimTK::State& s, const Vec3 f1_G = (forceMag/d) * r_G; - bodyForces[_b1->getMobilizedBodyIndex()] += SpatialVec(s1_G % f1_G, f1_G); - bodyForces[_b2->getMobilizedBodyIndex()] -= SpatialVec(s2_G % f1_G, f1_G); + forceConsumer.consumeBodySpatialVec(s, *_body1, SpatialVec(s1_G % f1_G, f1_G)); + forceConsumer.consumeBodySpatialVec(s, *_body2, -SpatialVec(s2_G % f1_G, f1_G)); } // get the force magnitude that has already been computed diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h index 67a3929d13..38943e8148 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h @@ -23,7 +23,8 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -#include "Force.h" +#include + #include namespace SimTK { @@ -52,8 +53,8 @@ namespace OpenSim { * * @author Ajay Seth */ -class OSIMSIMULATION_API ExpressionBasedPointToPointForce : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); +class OSIMSIMULATION_API ExpressionBasedPointToPointForce : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, ForceProducer); public: //============================================================================== // PROPERTIES @@ -140,16 +141,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); const double& getForceMagnitude(const SimTK::State& state) const; - //-------------------------------------------------------------------------- - // COMPUTATION - //-------------------------------------------------------------------------- - /** Compute the point-to-point force based on the user-defined expression - and apply it to the model */ - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - - //----------------------------------------------------------------------------- // Reporting //----------------------------------------------------------------------------- @@ -171,6 +162,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); void extendAddToSystem(SimTK::MultibodySystem& system) const override; private: + /** + * Implements the `ForceProducer` API by computing the point-to-point force + * based on the `expression` property. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void setNull(); void constructProperties(); @@ -180,8 +177,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); // Temporary solution until implemented with Sockets SimTK::ReferencePtr _body1; SimTK::ReferencePtr _body2; - SimTK::ReferencePtr _b1; - SimTK::ReferencePtr _b2; mutable CacheVariable _forceMagnitudeCV; diff --git a/OpenSim/Simulation/Model/ExternalForce.cpp b/OpenSim/Simulation/Model/ExternalForce.cpp index 11b4576bb0..eb6608ca77 100644 --- a/OpenSim/Simulation/Model/ExternalForce.cpp +++ b/OpenSim/Simulation/Model/ExternalForce.cpp @@ -24,6 +24,7 @@ //============================================================================== // INCLUDES //============================================================================== +#include #include #include #include @@ -51,7 +52,7 @@ using namespace std; /** * Default constructor. */ -ExternalForce::ExternalForce() : Force() +ExternalForce::ExternalForce() { setNull(); constructProperties(); @@ -328,9 +329,8 @@ void ExternalForce::extendConnectToModel(Model& model) //----------------------------------------------------------------------------- //_____________________________________________________________________________ -void ExternalForce::computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void ExternalForce::implProduceForces(const SimTK::State& state, + ForceConsumer& forceConsumer) const { double time = state.getTime(); @@ -345,13 +345,14 @@ void ExternalForce::computeForce(const SimTK::State& state, point = _pointExpressedInBody-> findStationLocationInAnotherFrame(state, point, *_appliedToBody); } - applyForceToPoint(state, *_appliedToBody, point, force, bodyForces); + + forceConsumer.consumePointForce(state, *_appliedToBody, point, force); } if (_appliesTorque) { Vec3 torque = getTorqueAtTime(time); torque = _forceExpressedInBody->expressVectorInGround(state, torque); - applyTorque(state, *_appliedToBody, torque, bodyForces); + forceConsumer.consumeTorque(state, *_appliedToBody, torque); } } diff --git a/OpenSim/Simulation/Model/ExternalForce.h b/OpenSim/Simulation/Model/ExternalForce.h index 99fe159e35..ce199deb9e 100644 --- a/OpenSim/Simulation/Model/ExternalForce.h +++ b/OpenSim/Simulation/Model/ExternalForce.h @@ -23,7 +23,8 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ // INCLUDE -#include "Force.h" + +#include namespace OpenSim { @@ -52,8 +53,8 @@ class Function; * * @author Ajay Seth */ -class OSIMSIMULATION_API ExternalForce : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(ExternalForce, Force); +class OSIMSIMULATION_API ExternalForce : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(ExternalForce, ForceProducer); public: //============================================================================== // PROPERTIES @@ -220,11 +221,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExternalForce, Force); void extendConnectToModel(Model& model) override; /** - * Compute the force. + * Implements the `ForceProducer` API. */ - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; private: void setNull(); diff --git a/OpenSim/Simulation/Model/ExternalLoads.cpp b/OpenSim/Simulation/Model/ExternalLoads.cpp index 686c287a30..2b1d8e3257 100644 --- a/OpenSim/Simulation/Model/ExternalLoads.cpp +++ b/OpenSim/Simulation/Model/ExternalLoads.cpp @@ -76,6 +76,12 @@ ExternalLoads::ExternalLoads(const ExternalLoads &otherExternalLoads) : ModelComponentSet(otherExternalLoads), _dataFileName(_dataFileNameProp.getValueStr()) { + // copy the document over, because it's used during `extendFinalizeConnections` + // to figure out where the associated motion file (#3926) + if (auto* document = otherExternalLoads.getDocument()) { + setDocument(std::make_unique(*document).release()); + } + setNull(); // Class Members diff --git a/OpenSim/Simulation/Model/ForceApplier.cpp b/OpenSim/Simulation/Model/ForceApplier.cpp new file mode 100644 index 0000000000..27e9805198 --- /dev/null +++ b/OpenSim/Simulation/Model/ForceApplier.cpp @@ -0,0 +1,45 @@ +#include "ForceApplier.h" + +#include +#include +#include +#include + +void OpenSim::ForceApplier::implConsumeGeneralizedForce( + const SimTK::State& state, + const Coordinate& coord, + double force) +{ + _matter->addInMobilityForce( + state, + SimTK::MobilizedBodyIndex(coord.getBodyIndex()), + SimTK::MobilizerUIndex(coord.getMobilizerQIndex()), + force, + *_generalizedForces + ); +} + +void OpenSim::ForceApplier::implConsumeBodySpatialVec( + const SimTK::State& state, + const PhysicalFrame& body, + const SimTK::SpatialVec& spatialVec) +{ + const auto mobilizedBodyIndex = body.getMobilizedBodyIndex(); + OPENSIM_ASSERT_ALWAYS(0 <= mobilizedBodyIndex && mobilizedBodyIndex < _bodyForces->size() && "the provided mobilized body index is out-of-bounds"); + (*_bodyForces)[mobilizedBodyIndex] += spatialVec; +} + +void OpenSim::ForceApplier::implConsumePointForce( + const SimTK::State& state, + const PhysicalFrame& frame, + const SimTK::Vec3& point, + const SimTK::Vec3& force) +{ + _matter->addInStationForce( + state, + frame.getMobilizedBodyIndex(), + frame.findTransformInBaseFrame() * point, + force, + *_bodyForces + ); +} \ No newline at end of file diff --git a/OpenSim/Simulation/Model/ForceApplier.h b/OpenSim/Simulation/Model/ForceApplier.h new file mode 100644 index 0000000000..05df8c2290 --- /dev/null +++ b/OpenSim/Simulation/Model/ForceApplier.h @@ -0,0 +1,87 @@ +#ifndef OPENSIM_FORCE_APPLIER_H_ +#define OPENSIM_FORCE_APPLIER_H_ + +/* -------------------------------------------------------------------------- * + * OpenSim: ForceApplier.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Adam Kewley * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include + +#include +#include + +#include // for `SimTK::SpatialVec` +#include // for `SimTK::Vector_` + +namespace SimTK { class SimbodyMatterSubsystem; } + +namespace OpenSim +{ + +/** + * A `ForceApplier` is a concrete `ForceConsumer` implementation that: + * + * - Converts any point forces into body forces + * - Applies body forces to a `SimTK::Vector_` + * - Applies generalized (mobility) forces to a `SimTK::Vector` + * + * The `ForceApplier` is primarily used as an internal class for adapting + * the `OpenSim::ForceConsumer`'s API contract ("undefined virtual + * consumption") to the `OpenSim::Force`'s API contract ("applies forces + * to a multibody system"). + */ +class OSIMSIMULATION_API ForceApplier final : public ForceConsumer { +public: + /** + * Constructs a `ForceApplier` that applies forces generated for `matter` to + * the `bodyForces` and `generalizedForces` vectors, if provided. + * + * Throws if any of `matter`, `bodyForces`, or `generalizedForces` are null. + */ + explicit ForceApplier( + const SimTK::SimbodyMatterSubsystem* matter, + SimTK::Vector_* bodyForces, + SimTK::Vector* generalizedForces) : + + _matter{matter}, + _bodyForces{bodyForces}, + _generalizedForces{generalizedForces} + { + OPENSIM_ASSERT_ALWAYS(matter != nullptr && "the matter argument cannot be null"); + OPENSIM_ASSERT_ALWAYS(bodyForces != nullptr && "the body forces argument cannot be null"); + OPENSIM_ASSERT_ALWAYS(generalizedForces != nullptr && "the generalized forces vector cannot be null"); + } + +private: + + void implConsumeGeneralizedForce(const SimTK::State&, const Coordinate&, double) final; + void implConsumeBodySpatialVec(const SimTK::State&, const PhysicalFrame&, const SimTK::SpatialVec&) final; + void implConsumePointForce(const SimTK::State&, const PhysicalFrame&, const SimTK::Vec3&, const SimTK::Vec3&) final; + + const SimTK::SimbodyMatterSubsystem* _matter; + SimTK::Vector_* _bodyForces; + SimTK::Vector* _generalizedForces; +}; + +} // namespace OpenSim + +#endif // OPENSIM_FORCE_APPLIER_H_ \ No newline at end of file diff --git a/OpenSim/Simulation/Model/ForceConsumer.h b/OpenSim/Simulation/Model/ForceConsumer.h new file mode 100644 index 0000000000..48ff32848d --- /dev/null +++ b/OpenSim/Simulation/Model/ForceConsumer.h @@ -0,0 +1,186 @@ +#ifndef OPENSIM_FORCE_CONSUMER_H_ +#define OPENSIM_FORCE_CONSUMER_H_ + +/* -------------------------------------------------------------------------- * + * OpenSim: ForceConsumer.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Adam Kewley * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include // for `SimTK::Vec3` +#include // for `SimTK::SpatialVec` + +namespace OpenSim { class Coordinate; } +namespace OpenSim { class PhysicalFrame; } +namespace SimTK { class State; } + +namespace OpenSim +{ + +/** +* A `ForceConsumer` is an abstract class that can consume forces via its `consume*` +* methods. It is typically used in conjunction with `ForceProducer`s, which produce +* the forces that this API consumes. +* +* The `ForceConsumer` API does not dictate how concrete implementations should +* handle the forces. This is to support several use-cases (examples): +* +* - Implementations that actually apply the forces (see: `ForceProducer::computeForce`, `ForceApplier`), +* - Implementations that print force debugging information +* - Implementations that want to render force vectors in 3D (e.g. UIs) +*/ +class ForceConsumer { +protected: + ForceConsumer() = default; + ForceConsumer(const ForceConsumer&) = default; + ForceConsumer(ForceConsumer&&) noexcept = default; + ForceConsumer& operator=(const ForceConsumer&) = default; + ForceConsumer& operator=(ForceConsumer&&) noexcept = default; + +public: + virtual ~ForceConsumer() noexcept = default; + + /** + * Consumes a generalized force. + * + * This is the `ForceConsumer`'s dual to `OpenSim::Force::applyGeneralizedForce` + * + * @param state the state that was used to evaluate the force + * @param coord the generalized coordinate to which the force applies + * @param force the (scalar) force change in the generalized coordinate + */ + void consumeGeneralizedForce( + const SimTK::State& state, + const Coordinate& coord, + double force) + { + implConsumeGeneralizedForce(state, coord, force); + } + + /** + * Consumes a body torque (index 0) and force (index 1) as a `SimTK::SpatialVec` + * + * If callers to this function are handling point-force-like data, then they should + * prefer calling `consumePointForce` and `consumeTorque` seperately instead of + * calling this. This so that concrete `ForceConsumer` implementations are able + * to introspect the point forces, rather than only receiving fully-resolved body + * forces. + * + * @param state the state that was used to evaluate the force + * @param body the body (frame) to which the force applies (at its center) + * @param spatialVec a `SimTK::SpatialVector` that contains the torque at index 0 and force at index 1 + */ + void consumeBodySpatialVec( + const SimTK::State& state, + const PhysicalFrame& body, + const SimTK::SpatialVec& spatialVec) + { + implConsumeBodySpatialVec(state, body, spatialVec); + } + + /** + * Consumes a body torque. + * + * This is the `ForceConsumer`'s dual to `OpenSim::Force::applyTorque`. + * + * If a caller to this function has both a body torque and a body (not point) force + * available, then it should prefer calling `consumeBodySpatialVec`. + * + * @param state the state that was used to evaluate the torque + * @param body the body to which the torque applies + * @param torque the torque vector, specified in the inertial frame + */ + void consumeTorque( + const SimTK::State& state, + const PhysicalFrame& body, + const SimTK::Vec3& torque) + { + consumeBodySpatialVec(state, body, SimTK::SpatialVec{torque, SimTK::Vec3{0.0, 0.0, 0.0}}); + } + + /** + * Consumes a point force. That is, a force applied at a point (a "station") within + * a frame. + * + * This is the `ForceConsumer`'s dual to `OpenSim::Force::applyForceToPoint` + * + * @param state the state that was used to evaluate the force + * @param frame the frame in which `point` is defined + * @param point a point in `frame` where `force` applies + * @param force the force vector, specified in the inertial (ground) frame + */ + void consumePointForce( + const SimTK::State& state, + const PhysicalFrame& frame, + const SimTK::Vec3& point, + const SimTK::Vec3& force) + { + implConsumePointForce(state, frame, point, force); + } + +private: + /** + * Subclasses of `ForceConsumer` may implement this method. There are no expectations + * on how an implementation should handle a call to this function. + * + * @param state the state that was used to evaluate the force + * @param coord the generalized coordinate to which the force applies + * @param force the (scalar) force change in the generalized coordinate + */ + virtual void implConsumeGeneralizedForce( + const SimTK::State& state, + const Coordinate& coord, + double force) + {} + + /** + * Subclasses of `ForceConsumer` may implement this method. There are no expectations + * on how an implementation should handle a call to this function. + * + * @param state the state that was used to evaluate the force + * @param body the body (frame) to which the force applies (at its center) + * @param spatialVec a `SimTK::SpatialVector` that contains the torque at index 0 and force at index 1 + */ + virtual void implConsumeBodySpatialVec( + const SimTK::State& state, + const PhysicalFrame& body, + const SimTK::SpatialVec& spatialVec) + {} + + /** + * Subclasses of `ForceConsumer` may implement this method. There are no expectations + * on how an implementation should handle a call to this function. + * + * @param state the state that was used to evaluate the force + * @param frame the frame in which `point` is defined + * @param point a point in `frame` where `force` applies + * @param force the force vector, specified in the inertial (ground) frame + */ + virtual void implConsumePointForce( + const SimTK::State& state, + const PhysicalFrame& frame, + const SimTK::Vec3& point, + const SimTK::Vec3& force) + {} +}; + +} // namespace OpenSim + +#endif // OPENSIM_FORCE_CONSUMER_H_ diff --git a/OpenSim/Simulation/Model/ForceProducer.cpp b/OpenSim/Simulation/Model/ForceProducer.cpp new file mode 100644 index 0000000000..ad8180fe95 --- /dev/null +++ b/OpenSim/Simulation/Model/ForceProducer.cpp @@ -0,0 +1,48 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ForceProducer.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Adam Kewley * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "ForceProducer.h" + +#include +#include + +using namespace OpenSim; + +void OpenSim::ForceProducer::computeForce( + const SimTK::State& state, + SimTK::Vector_& bodyForces, + SimTK::Vector& generalizedForces) const +{ + if (!appliesForce(state)) { + // This `Force` has explicitly stated that it doesn't want to apply + // the forces. + return; + } + + // create a consumer that uses each produced force to compute the + // underlying body- and generalized-forces + ForceApplier forceApplier{&_model->getMatterSubsystem(), &bodyForces, &generalizedForces}; + + // produce forces and feed them into the consumer, satisfying the `computeForce` API + produceForces(state, forceApplier); +} diff --git a/OpenSim/Simulation/Model/ForceProducer.h b/OpenSim/Simulation/Model/ForceProducer.h new file mode 100644 index 0000000000..9f8db609d0 --- /dev/null +++ b/OpenSim/Simulation/Model/ForceProducer.h @@ -0,0 +1,105 @@ +#ifndef OPENSIM_FORCE_PRODUCER_H_ +#define OPENSIM_FORCE_PRODUCER_H_ + +/* -------------------------------------------------------------------------- * + * OpenSim: ForceProducer.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Adam Kewley * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include +#include +#include + +namespace OpenSim { class ForceConsumer; } +namespace SimTK { class State; } + +namespace OpenSim { + +/** + * A `ForceProducer` is an abstract `OpenSim::Force` that can emit (produce) + * its forces one-by-one into a virtual `OpenSim::ForceConsumer`. + * + * The benefit of this is that it enables arbitrary external code to directly + * introspect each force before it gets resolved to the underlying body-/generalized-force + * vector that `OpenSim::Force::computeForce` manipulates. This can be useful for + * visualizing/dumping user data (e.g. because user-written `OpenSim::ExternalForce`s + * produce point-forces) or debugging (because it's easier to debug forces if they + * come one-at-a-time rather than trying to figure out which parts of downstream code + * touched which parts of a `SimTK::Vector_` during + * `OpenSim::Force::computeForce`). + */ +class OSIMSIMULATION_API ForceProducer : public Force { + OpenSim_DECLARE_ABSTRACT_OBJECT(ForceProducer, Force); + +protected: + using Force::Force; // forward the `Force` constructor + +public: + + /** + * Uses `implProduceForces` to produce (emit) forces evaluated from `state` into the + * provided `ForceConsumer`. + * + * @note this function only produces the forces and does not apply them to anything. It's + * up to the `ForceConsumer` implementation to handle the forces. Therefore, + * `Force::appliesForces` is ignored by this method. + * + * @param state the state used to evaluate forces + * @param consumer a `ForceConsumer` that shall receive each of the produced forces + */ + void produceForces(const SimTK::State& state, ForceConsumer& forceConsumer) const + { + implProduceForces(state, forceConsumer); + } + + /** + * Inhereted from `OpenSim::Force`. + * + * `ForceProducer` overrides `OpenSim::Force::computeForce` with a default + * implementation that, provided `OpenSim::Force::appliesForces` is `true`, + * internally uses `produceForces` to mutate the provided `bodyForces` in a + * manner that's compatible with the `OpenSim::Force` API. + */ + void computeForce( + const SimTK::State& state, + SimTK::Vector_& bodyForces, + SimTK::Vector& generalizedForces + ) const override; + +private: + + /** + * Subclasses of `ForceProducer` must implement this method. + * + * Implementations should evaluate forces from `state` and pass them into + * `forceConsumer` by calling the most appropriate `consume*` function. The + * `ForceConsumer`'s API documentation outlines each available consumption + * function (+ preferred usage). + */ + virtual void implProduceForces( + const SimTK::State& state, + ForceConsumer& forceConsumer + ) const = 0; +}; + +} + +#endif // OPENSIM_FORCE_PRODUCER_H_ diff --git a/OpenSim/Simulation/Model/FunctionBasedBushingForce.cpp b/OpenSim/Simulation/Model/FunctionBasedBushingForce.cpp index 0fd196b3e6..6805bd58e9 100644 --- a/OpenSim/Simulation/Model/FunctionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedBushingForce.cpp @@ -199,11 +199,9 @@ SimTK::Vec6 FunctionBasedBushingForce:: return -_dampingMatrix * dqdot; } -/* Compute the force contribution to the system and add in to appropriate - * bodyForce and/or system generalizedForce. */ -void FunctionBasedBushingForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void FunctionBasedBushingForce::implProduceForces( + const SimTK::State& s, + ForceConsumer& forceConsumer) const { // stiffness force Vec6 fk = calcStiffnessForce(s); @@ -215,7 +213,7 @@ void FunctionBasedBushingForce::computeForce(const SimTK::State& s, // convert internal forces to spatial and add then add to system // physical (body) forces - addInPhysicalForcesFromInternal(s, f, bodyForces); + producePhysicalForcesFromInternal(s, f, forceConsumer); } //============================================================================= diff --git a/OpenSim/Simulation/Model/FunctionBasedBushingForce.h b/OpenSim/Simulation/Model/FunctionBasedBushingForce.h index 0f047c13ee..041300cb1c 100644 --- a/OpenSim/Simulation/Model/FunctionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/FunctionBasedBushingForce.h @@ -25,7 +25,7 @@ // INCLUDE -#include "Force.h" +#include #include namespace OpenSim { @@ -50,7 +50,7 @@ class Function; * @author Matt DeMers */ class OSIMSIMULATION_API FunctionBasedBushingForce - : public TwoFrameLinker { + : public TwoFrameLinker { OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedBushingForce, TwoFrameLinker); public: //============================================================================== @@ -165,13 +165,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedBushingForce, TwoFrameLinker); force on frame2 from frame1 in the basis of the deflection rate (dqdot).*/ SimTK::Vec6 calcDampingForce(const SimTK::State& state) const; - /** Compute the bushing force contribution to the system and add in to appropriate - * bodyForce and/or system generalizedForce. - */ - void computeForce (const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - //-------------------------------------------------------------------------- // Reporting //-------------------------------------------------------------------------- @@ -194,7 +187,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedBushingForce, TwoFrameLinker); const ModelDisplayHints& hints, const SimTK::State& state, SimTK::Array_& geometryArray) const override; + private: + /** + * Implements `ForceProducer` by computing the bushing force contribution and + * sending it to the supplied `ForceConsumer` + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; void setNull(); void constructProperties(); diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index b4fd6f861e..ae7ed17e5c 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -25,6 +25,7 @@ #include "Model.h" #include +#include using namespace OpenSim; @@ -137,36 +138,28 @@ double FunctionBasedPath::computeMomentArm(const SimTK::State& s, } } -double FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const -{ - computeLengtheningSpeed(s); - return getCacheVariableValue(s, _lengtheningSpeedCV); -} - -void FunctionBasedPath::addInEquivalentForces(const SimTK::State& state, - const double& tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const +void FunctionBasedPath::produceForces(const SimTK::State& state, + double tension, + ForceConsumer& forceConsumer) const { // Get the moment arms. computeMomentArms(state); const auto& momentArms = - getCacheVariableValue(state, _momentArmsCV); + getCacheVariableValue(state, _momentArmsCV); OPENSIM_ASSERT_ALWAYS(momentArms.size() == (int)_coordinates.size()); - // Apply the mobility forces. - const SimTK::SimbodyMatterSubsystem& matter = - getModel().getMatterSubsystem(); + // Produce mobility forces for (int i = 0; i < (int)_coordinates.size(); ++i) { - const SimTK::MobilizedBody& mobod = - matter.getMobilizedBody(_coordinates[i]->getBodyIndex()); - mobod.applyOneMobilityForce(state, - _coordinates[i]->getMobilizerQIndex(), - momentArms[i] * tension, - mobilityForces); + forceConsumer.consumeGeneralizedForce(state, *_coordinates[i], momentArms[i] * tension); } } +double FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const +{ + computeLengtheningSpeed(s); + return getCacheVariableValue(s, _lengtheningSpeedCV); +} + //============================================================================= // CONVENIENCE METHODS //============================================================================= diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index 6dddf389b0..13b6b8db25 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -186,10 +186,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, AbstractGeometryPath); /// SimTK::State, use `getMomentArms()` instead. double computeMomentArm(const SimTK::State& s, const Coordinate& coord) const override; - void addInEquivalentForces(const SimTK::State& state, - const double& tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + + void produceForces(const SimTK::State&, + double tension, + ForceConsumer&) const override; + bool isVisualPath() const override { return false; } private: diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 177af24592..b5c7e3b891 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -25,12 +25,13 @@ // INCLUDES //============================================================================= #include "GeometryPath.h" -#include "ConditionalPathPoint.h" -#include "MovingPathPoint.h" -#include "PointForceDirection.h" -#include "Model.h" #include +#include +#include +#include +#include +#include #include //============================================================================= @@ -128,6 +129,24 @@ static void PopulatePathElementLookup( } } +// Returns a normalized direction vector from `a` to `b` in ground, or a vector containing `NaN`s +// if `a` and `b` are at the same location. +// +// Two points can be conincient due to infeasible wrapping of the path. E.g. when the origin or +// insertion enters the wrapping surface. This is a temporary fix, since the wrap algorithm +// should return NaN for the points and/or throw an Exception- aseth +static SimTK::Vec3 directionBetweenPointsInGroundOrNaNIfCoincident(const SimTK::State& s, const OpenSim::AbstractPathPoint& a, const OpenSim::AbstractPathPoint& b) +{ + Vec3 dir = b.getLocationInGround(s) - a.getLocationInGround(s); + if (dir.norm() < SimTK::SignificantReal) { + dir = dir*SimTK::NaN; + } + else { + dir = dir.normalize(); + } + return dir; +} + //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= @@ -381,125 +400,59 @@ getPointForceDirections(const SimTK::State& s, } } -/* add in the equivalent spatial forces on bodies for an applied tension - along the GeometryPath to a set of bodyForces */ -void GeometryPath::addInEquivalentForces(const SimTK::State& s, - const double& tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const +void GeometryPath::produceForces(const SimTK::State& s, + double tension, + ForceConsumer& forceConsumer) const { - AbstractPathPoint* start = NULL; - AbstractPathPoint* end = NULL; - const SimTK::MobilizedBody* bo = NULL; - const SimTK::MobilizedBody* bf = NULL; - const Array& currentPath = getCurrentPath(s); - int np = currentPath.getSize(); - - const SimTK::SimbodyMatterSubsystem& matter = - getModel().getMatterSubsystem(); - - // start point, end point, direction, and force vectors in ground - Vec3 po(0), pf(0), dir(0), force(0); - // partial velocity of point in body expressed in ground - Vec3 dPodq_G(0), dPfdq_G(0); - - // gen force (torque) due to moving point under tension - double fo, ff; - - for (int i = 0; i < np-1; ++i) { - start = currentPath[i]; - end = currentPath[i+1]; - - bo = &start->getParentFrame().getMobilizedBody(); - bf = &end->getParentFrame().getMobilizedBody(); - - if (bo != bf) { - // Find the positions of start and end in the inertial frame. - po = start->getLocationInGround(s); - pf = end->getLocationInGround(s); - - // Form a vector from start to end, in the inertial frame. - dir = (pf - po); - - // Check that the two points are not coincident. - // This can happen due to infeasible wrapping of the path, - // when the origin or insertion enters the wrapping surface. - // This is a temporary fix, since the wrap algorithm should - // return NaN for the points and/or throw an Exception- aseth - if (dir.norm() < SimTK::SignificantReal){ - dir = dir*SimTK::NaN; - } - else{ - dir = dir.normalize(); - } - - force = tension*dir; + // Retains the body index from the previous iteration of the main loop + // and the previous direction between the previous point and the current + // one (if applicable). This is used to ensure that only one point force + // is produced per path point (#3903, #3891). + MobilizedBodyIndex previousBodyIndex = MobilizedBodyIndex::Invalid(); + SimTK::Vec3 previousDirection(0.0); - const MovingPathPoint* mppo = - dynamic_cast(start); + const Array& currentPath = getCurrentPath(s); + for (int i = 0; i < currentPath.getSize(); ++i) { - // do the same for the end point of this segment of the path - const MovingPathPoint* mppf = - dynamic_cast(end); + const AbstractPathPoint& currentPoint = *currentPath[i]; + const MobilizedBodyIndex currentBodyIndex = currentPoint.getParentFrame().getMobilizedBodyIndex(); - // add in the tension point forces to body forces - if (mppo) {// moving path point location is a function of the state - // transform of the frame of the point to the base mobilized body - auto X_BF = mppo->getParentFrame().findTransformInBaseFrame(); - bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force, - bodyForces); - } - else { - // transform of the frame of the point to the base mobilized body - auto X_BF = start->getParentFrame().findTransformInBaseFrame(); - bo->applyForceToBodyPoint(s, X_BF*start->getLocation(s), force, - bodyForces); - } + SimTK::Vec3 force(0.0); + if (previousBodyIndex.isValid() && currentBodyIndex != previousBodyIndex) { + force -= tension * previousDirection; + } - if (mppf) {// moving path point location is a function of the state - // transform of the frame of the point to the base mobilized body - auto X_BF = mppf->getParentFrame().findTransformInBaseFrame(); - bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force, - bodyForces); + const AbstractPathPoint* nextPoint = i < currentPath.getSize()-1 ? currentPath[i+1] : nullptr; + if (nextPoint && nextPoint->getParentFrame().getMobilizedBodyIndex() != currentBodyIndex) { + const SimTK::Vec3 currentDirection = directionBetweenPointsInGroundOrNaNIfCoincident(s, currentPoint, *nextPoint); + const SimTK::Vec3 currentToNextForce = tension * currentDirection; + force += currentToNextForce; + + // Additionally, account for the work done due to the movement of a `MovingPathPoint` + // relative to the body it is on. + if (const auto* movingCurrentPoint = dynamic_cast(¤tPoint)) { + const Vec3 dPodq_G = currentPoint.getParentFrame().expressVectorInGround(s, currentPoint.getdPointdQ(s)); + const double fo = ~dPodq_G*currentToNextForce; + forceConsumer.consumeGeneralizedForce(s, movingCurrentPoint->getXCoordinate(), fo); } - else { - // transform of the frame of the point to the base mobilized body - auto X_BF = end->getParentFrame().findTransformInBaseFrame(); - bf->applyForceToBodyPoint(s, X_BF*end->getLocation(s), -force, - bodyForces); + if (const auto* nextMovingPoint = dynamic_cast(nextPoint)) { + const Vec3 dPfdq_G = nextPoint->getParentFrame().expressVectorInGround(s, nextPoint->getdPointdQ(s)); + const double ff = ~dPfdq_G*(-currentToNextForce); + forceConsumer.consumeGeneralizedForce(s, nextMovingPoint->getXCoordinate(), ff); } - // Now account for the work being done by virtue of the moving - // path point motion relative to the body it is on - if(mppo){ - // torque (genforce) contribution due to relative movement - // of a via point w.r.t. the body it is connected to. - dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s)); - fo = ~dPodq_G*force; - - // get the mobilized body the coordinate is couple to. - const SimTK::MobilizedBody& mpbod = - matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex()); - - // apply the generalized (mobility) force to the coordinate's body - mpbod.applyOneMobilityForce(s, - mppo->getXCoordinate().getMobilizerQIndex(), - fo, mobilityForces); - } - - if(mppf){ - dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s)); - ff = ~dPfdq_G*(-force); - - // get the mobilized body the coordinate is couple to. - const SimTK::MobilizedBody& mpbod = - matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex()); + previousBodyIndex = currentBodyIndex; + previousDirection = currentDirection; + } + else { + previousBodyIndex = MobilizedBodyIndex::Invalid(); + previousDirection = SimTK::Vec3(0.0f); + } - mpbod.applyOneMobilityForce(s, - mppf->getXCoordinate().getMobilizerQIndex(), - ff, mobilityForces); - } - } + // If applicable, produce the force + if (force != SimTK::Vec3(0.0)) { + forceConsumer.consumePointForce(s, currentPoint.getParentFrame(), currentPoint.getLocation(s), force); + } } } diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index cbaf7ba517..05bbc029e1 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -147,17 +147,17 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, AbstractGeometryPath); void getPointForceDirections(const SimTK::State& s, OpenSim::Array *rPFDs) const; - /** add in the equivalent body and generalized forces to be applied to the - multibody system resulting from a tension along the GeometryPath - @param state state used to evaluate forces - @param[in] tension scalar (double) of the applied (+ve) tensile force - @param[in,out] bodyForces Vector of SpatialVec's (torque, force) on bodies - @param[in,out] mobilityForces Vector of generalized forces, one per mobility - */ - void addInEquivalentForces(const SimTK::State& state, - const double& tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; + /** + * Requests forces resulting from applying a tension along its path and + * emits them into the supplied `ForceConsumer`. + * + * @param state the state used to evaluate forces + * @param tension scalar of the applied (+ve) tensile force + * @param forceConsumer a `ForceConsumer` shall receive each produced force + */ + void produceForces(const SimTK::State& state, + double tension, + ForceConsumer& forceConsumer) const override; bool isVisualPath() const override { return true; } diff --git a/OpenSim/Simulation/Model/Ligament.cpp b/OpenSim/Simulation/Model/Ligament.cpp index 351c22aeaa..b05cd5c549 100644 --- a/OpenSim/Simulation/Model/Ligament.cpp +++ b/OpenSim/Simulation/Model/Ligament.cpp @@ -123,10 +123,8 @@ SimTK::Vec3 Ligament::computePathColor(const SimTK::State& state) const { void Ligament::extendAddToSystem(SimTK::MultibodySystem& system) const { Super::extendAddToSystem(system); - // Cache the computed tension and strain of the ligament - this->_tensionCV = addCacheVariable("tension", 0.0, SimTK::Stage::Velocity); - this->_strainCV = addCacheVariable("strain", 0.0, SimTK::Stage::Velocity); + this->_tensionCV = addCacheVariable("tension", 0.0, SimTK::Stage::Position); } @@ -204,9 +202,28 @@ void Ligament::extendPostScale(const SimTK::State& s, const ScaleSet& scaleSet) } } -const double& Ligament::getTension(const SimTK::State& s) const +double Ligament::getTension(const SimTK::State& s) const { - return getCacheVariableValue(s, _tensionCV); + if (isCacheVariableValid(s, _tensionCV)) { + return getCacheVariableValue(s, _tensionCV); + } + // else: compute a new tension + + const auto& path = getPath(); + const double restingLength = get_resting_length(); + const auto pathLength = path.getLength(s); + + if (pathLength <= restingLength) { + setCacheVariableValue(s, _tensionCV, 0.0); + return 0.0; + } + else { + // evaluate normalized tendon force length curve + const double tension = getForceLengthCurve().calcValue( + SimTK::Vector(1, pathLength/restingLength)) * get_pcsa_force(); + setCacheVariableValue(s, _tensionCV, tension); + return tension; + } } @@ -221,28 +238,8 @@ double Ligament::computeMomentArm(const SimTK::State& s, Coordinate& aCoord) con return getPath().computeMomentArm(s, aCoord); } - - -void Ligament::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void Ligament::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const { - const auto& path = getPath(); - const double& restingLength = get_resting_length(); - const double& pcsaForce = get_pcsa_force(); - - double tension = 0; - - if (path.getLength(s) <= restingLength){ - setCacheVariableValue(s, _tensionCV, tension); - return; - } - - // evaluate normalized tendon force length curve - tension = getForceLengthCurve().calcValue( - SimTK::Vector(1, path.getLength(s)/restingLength))* pcsaForce; - setCacheVariableValue(s, _tensionCV, tension); - - path.addInEquivalentForces(s, tension, bodyForces, generalizedForces); + getPath().produceForces(s, getTension(s), forceConsumer); } - diff --git a/OpenSim/Simulation/Model/Ligament.h b/OpenSim/Simulation/Model/Ligament.h index 26da4952f8..08a7d9ab95 100644 --- a/OpenSim/Simulation/Model/Ligament.h +++ b/OpenSim/Simulation/Model/Ligament.h @@ -27,9 +27,10 @@ //============================================================================= // INCLUDES //============================================================================= -#include "AbstractGeometryPath.h" -#include "Force.h" -#include "GeometryPath.h" + +#include +#include +#include #ifdef SWIG #ifdef OSIMACTUATORS_API @@ -49,8 +50,8 @@ class ScaleSet; * A class implementing a ligament. The path of the ligament is * stored in an object derived from AbstractGeometryPath. */ -class OSIMSIMULATION_API Ligament : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(Ligament, Force); +class OSIMSIMULATION_API Ligament : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(Ligament, ForceProducer); public: //============================================================================= // PROPERTIES @@ -121,15 +122,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Ligament, Force); virtual bool setForceLengthCurve(const Function& aForceLengthCurve); // computed variables - const double& getTension(const SimTK::State& s) const; + double getTension(const SimTK::State& s) const; - //-------------------------------------------------------------------------- - // COMPUTATIONS - //-------------------------------------------------------------------------- virtual double computeMomentArm(const SimTK::State& s, Coordinate& aCoord) const; - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; //-------------------------------------------------------------------------- // SCALE @@ -191,10 +186,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Ligament, Force); } private: + void implProduceForces(const SimTK::State&, ForceConsumer&) const final; + void constructProperties(); mutable CacheVariable _tensionCV; - mutable CacheVariable _strainCV; //============================================================================= }; // END of class Ligament diff --git a/OpenSim/Simulation/Model/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp index 2689458d0d..ae3bcaa84d 100644 --- a/OpenSim/Simulation/Model/PathActuator.cpp +++ b/OpenSim/Simulation/Model/PathActuator.cpp @@ -159,35 +159,6 @@ double PathActuator::computeActuation(const SimTK::State& s) const return( getControl(s) * get_optimal_force() ); } - -//============================================================================= -// APPLICATION -//============================================================================= -//_____________________________________________________________________________ -/** - * Apply the actuator force along path wrapping over and connecting rigid bodies - */ -void PathActuator::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const -{ - if(!_model) return; - - const auto &path = getPath(); - - double force =0; - if( isActuationOverridden(s) ) { - force = computeOverrideActuation(s); - } else { - force = computeActuation(s); - } - - // the force of this actuator used to compute power - setActuation(s, force); - - path.addInEquivalentForces(s, force, bodyForces, mobilityForces); -} - /** * Compute the moment-arm of this muscle about a coordinate. */ @@ -213,6 +184,23 @@ void PathActuator::extendRealizeDynamics(const SimTK::State& state) const } } +void PathActuator::implProduceForces(const SimTK::State& s, + ForceConsumer& forceConsumer) const +{ + if (!_model) { + return; + } + + const double force = isActuationOverridden(s) ? + computeOverrideActuation(s) : + computeActuation(s); + + // the force of this actuator used to compute power + setActuation(s, force); + + getPath().produceForces(s, force, forceConsumer); +} + //------------------------------------------------------------------------------ // COMPUTE PATH COLOR //------------------------------------------------------------------------------ diff --git a/OpenSim/Simulation/Model/PathActuator.h b/OpenSim/Simulation/Model/PathActuator.h index 3e57e28df9..67fd93958c 100644 --- a/OpenSim/Simulation/Model/PathActuator.h +++ b/OpenSim/Simulation/Model/PathActuator.h @@ -132,13 +132,6 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator { const PhysicalFrame& aBody, const SimTK::Vec3& aPositionOnBody); - //-------------------------------------------------------------------------- - // APPLICATION - //-------------------------------------------------------------------------- - virtual void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const override; - //-------------------------------------------------------------------------- // COMPUTATIONS //-------------------------------------------------------------------------- @@ -169,6 +162,12 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator { void extendRealizeDynamics(const SimTK::State& state) const override; private: + /** + * Implements the `ForceProducer` API by applying the actuator force along + * the (potentially, wrapping) path connecting rigid bodies. + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void setNull(); void constructProperties(); diff --git a/OpenSim/Simulation/Model/PathSpring.cpp b/OpenSim/Simulation/Model/PathSpring.cpp index b00718ce32..4ab7daaf31 100644 --- a/OpenSim/Simulation/Model/PathSpring.cpp +++ b/OpenSim/Simulation/Model/PathSpring.cpp @@ -194,13 +194,7 @@ double PathSpring::computeMomentArm(const SimTK::State& s, return getPath().computeMomentArm(s, aCoord); } - - -void PathSpring::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void PathSpring::implProduceForces(const SimTK::State& s, ForceConsumer& forceConsumer) const { - const AbstractGeometryPath& path = getPath(); - const double& tension = getTension(s); - path.addInEquivalentForces(s, tension, bodyForces, generalizedForces); + getPath().produceForces(s, getTension(s), forceConsumer); } diff --git a/OpenSim/Simulation/Model/PathSpring.h b/OpenSim/Simulation/Model/PathSpring.h index e2878fa6d6..dd0e27be40 100644 --- a/OpenSim/Simulation/Model/PathSpring.h +++ b/OpenSim/Simulation/Model/PathSpring.h @@ -24,12 +24,9 @@ * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= -#include "AbstractGeometryPath.h" -#include "Force.h" -#include "GeometryPath.h" +#include +#include +#include namespace OpenSim { @@ -51,8 +48,8 @@ class ScaleSet; * * @author Ajay Seth */ -class OSIMSIMULATION_API PathSpring : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(PathSpring, Force); +class OSIMSIMULATION_API PathSpring : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(PathSpring, ForceProducer); public: //============================================================================= // PROPERTIES @@ -180,11 +177,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PathSpring, Force); const ScaleSet& scaleSet) override; protected: - /** Implementation of Force component virtual method */ - void computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; - /** Implement ModelComponent interface. */ void extendFinalizeFromProperties() override; @@ -210,6 +202,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PathSpring, Force); } private: + /** + * Implements the `ForceProducer` API + */ + void implProduceForces(const SimTK::State&, ForceConsumer&) const override; + void constructProperties(); //============================================================================= diff --git a/OpenSim/Simulation/Model/PointForceDirection.h b/OpenSim/Simulation/Model/PointForceDirection.h index af47c756fb..4296cfbb3e 100644 --- a/OpenSim/Simulation/Model/PointForceDirection.h +++ b/OpenSim/Simulation/Model/PointForceDirection.h @@ -9,8 +9,8 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2017 Stanford University and the Authors * - * Author(s): Ajay Seth * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Ajay Seth, Adam Kewley * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -23,70 +23,74 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -// INCLUDES +#include #include namespace OpenSim { class Body; class PhysicalFrame; -//============================================================================= -//============================================================================= -/** Convenience class for a generic representation of geometry of a complex - Force (or any other object) with multiple points of contact through - which forces are applied to bodies. This represents one such point and an - array of these objects defines a complete Force distribution (ie. path). + +/** + * Convenience class for a generic representation of geometry of a complex + * Force (or any other object) with multiple points of contact through + * which forces are applied to bodies. This represents one such point and an + * array of these objects defines a complete Force distribution (i.e., path). * * @author Ajay Seth * @version 1.0 */ +class OSIMSIMULATION_API PointForceDirection final { +public: + PointForceDirection( + SimTK::Vec3 point, + const PhysicalFrame& frame, + SimTK::Vec3 direction) : + + _point(point), _frame(&frame), _direction(direction) + {} + + [[deprecated("the 'scale' functionality should not be used in new code: OpenSim already assumes 'direction' is non-unit-length")]] + PointForceDirection( + SimTK::Vec3 point, + const PhysicalFrame& frame, + SimTK::Vec3 direction, + double scale) : + + _point(point), _frame(&frame), _direction(direction), _scale(scale) + {} + + /** Returns the point of "contact", defined in `frame()` */ + SimTK::Vec3 point() { return _point; } -class OSIMSIMULATION_API PointForceDirection -{ + /** Returns the frame in which `point()` is defined */ + const PhysicalFrame& frame() { return *_frame; } + + /** Returns the (potentially, non-unit-length) direction, defined in ground, of the force at `point()` */ + SimTK::Vec3 direction() { return _direction; } + + /** Returns the scale factor of the force */ + [[deprecated("this functionality should not be used in new code: OpenSim already assumes 'direction' is non-unit-length")]] + double scale() { return _scale; } + + /** Replaces the current direction with `direction + newDirection` */ + void addToDirection(SimTK::Vec3 newDirection) { _direction += newDirection; } -//============================================================================= -// MEMBER VARIABLES -//============================================================================= private: /** Point of "contact" with a body, defined in the body frame */ SimTK::Vec3 _point; + /** The frame in which the point is defined */ - const PhysicalFrame &_frame; + const PhysicalFrame* _frame; + /** Direction of the force at the point, defined in ground */ SimTK::Vec3 _direction; - /** Optional parameter to scale the force that results from a scalar - (tension) multiplies the direction */ - double _scale; -//============================================================================= -// METHODS -//============================================================================= - //-------------------------------------------------------------------------- - // CONSTRUCTION - //-------------------------------------------------------------------------- -public: - virtual ~PointForceDirection() {}; - /** Default constructor takes the point, body, direction and scale - as arguments */ - PointForceDirection(SimTK::Vec3 point, const PhysicalFrame &frame, - SimTK::Vec3 direction, double scale=1): - _point(point), _frame(frame), _direction(direction), _scale(scale) - {} - /** get point of "contact" with on a body defined in the body frame */ - SimTK::Vec3 point() {return _point; } - /** get the body in which the point is defined */ - const PhysicalFrame& frame() {return _frame; } - /** get direction of the force at the point defined in ground */ - SimTK::Vec3 direction() {return _direction; } - /** get the scale factor on the force */ - double scale() {return _scale; } - - /** replace the current direction with the resultant with a new direction */ - void addToDirection(SimTK::Vec3 newDirection) {_direction+=newDirection;} - -//============================================================================= -}; // END of class PointForceDirection -//============================================================================= + /** Deprecated parameter to scale the force that results from a scalar + (tension) multiplies the direction */ + double _scale = 1.0; +}; + } // namespace #endif // __PointForceDirection_h__ diff --git a/OpenSim/Simulation/Model/PrescribedForce.cpp b/OpenSim/Simulation/Model/PrescribedForce.cpp index 893fc7b168..8aa2325510 100644 --- a/OpenSim/Simulation/Model/PrescribedForce.cpp +++ b/OpenSim/Simulation/Model/PrescribedForce.cpp @@ -23,10 +23,12 @@ //============================================================================= // INCLUDES //============================================================================= -#include -#include #include "PrescribedForce.h" +#include +#include +#include + //============================================================================= // STATICS //============================================================================= @@ -240,9 +242,9 @@ void PrescribedForce::setPointFunctionNames //----------------------------------------------------------------------------- //_____________________________________________________________________________ -void PrescribedForce::computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const +void PrescribedForce::implProduceForces( + const SimTK::State& state, + ForceConsumer& forceConsumer) const { const bool pointIsGlobal = get_pointIsGlobal(); const bool forceIsGlobal = get_forceIsGlobal(); @@ -269,7 +271,7 @@ void PrescribedForce::computeForce(const SimTK::State& state, Vec3 point(0); // Default is body origin. if (hasPointFunctions) { - // Apply force to a specified point on the body. + // Calculate point force at a specified point on the body. point = Vec3(pointFunctions[0].calcValue(timeAsVector), pointFunctions[1].calcValue(timeAsVector), pointFunctions[2].calcValue(timeAsVector)); @@ -277,7 +279,8 @@ void PrescribedForce::computeForce(const SimTK::State& state, point = gnd.findStationLocationInAnotherFrame(state, point, frame); } - applyForceToPoint(state, frame, point, force, bodyForces); + + forceConsumer.consumePointForce(state, frame, point, force); } if (hasTorqueFunctions){ Vec3 torque(torqueFunctions[0].calcValue(timeAsVector), @@ -286,7 +289,7 @@ void PrescribedForce::computeForce(const SimTK::State& state, if (!forceIsGlobal) torque = frame.expressVectorInAnotherFrame(state, torque, gnd); - applyTorque(state, frame, torque, bodyForces); + forceConsumer.consumeTorque(state, frame, torque); } } @@ -390,7 +393,7 @@ OpenSim::Array PrescribedForce::getRecordValues(const SimTK::State& stat const bool pointSpecified = pointFunctions.getSize()==3; const bool appliesTorque = torqueFunctions.getSize()==3; - // This is bad as it duplicates the code in computeForce we'll cleanup after it works! + // This is bad as it duplicates the code in `implProduceForces` we'll cleanup after it works! const double time = state.getTime(); const SimTK::Vector timeAsVector(1, time); const PhysicalFrame& frame = diff --git a/OpenSim/Simulation/Model/PrescribedForce.h b/OpenSim/Simulation/Model/PrescribedForce.h index 4e2fcc3484..54213f1ad0 100644 --- a/OpenSim/Simulation/Model/PrescribedForce.h +++ b/OpenSim/Simulation/Model/PrescribedForce.h @@ -22,9 +22,10 @@ * See the License for the specific language governing permissions and * * limitations under the License. * * -------------------------------------------------------------------------- */ -// INCLUDE -#include "OpenSim/Common/FunctionSet.h" -#include "Force.h" + +#include +#include +#include namespace OpenSim { @@ -50,8 +51,8 @@ function of time. It is defined by three sets of functions, all of which are @author Peter Eastman, Matt DeMers **/ -class OSIMSIMULATION_API PrescribedForce : public Force { -OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedForce, Force); +class OSIMSIMULATION_API PrescribedForce : public ForceProducer { +OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedForce, ForceProducer); public: //============================================================================== // PROPERTIES @@ -266,11 +267,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedForce, Force); } protected: - /** Force interface. **/ - void computeForce - (const SimTK::State& state, - SimTK::Vector_& bodyForces, - SimTK::Vector& generalizedForces) const override; + /** + * Implements the `OpenSim::ForceProducer` interface. + */ + void implProduceForces( + const SimTK::State& state, + ForceConsumer& consumer + ) const override; //============================================================================== // DATA diff --git a/OpenSim/Simulation/Model/TwoFrameLinker.h b/OpenSim/Simulation/Model/TwoFrameLinker.h index 513051c38f..d321f4468d 100644 --- a/OpenSim/Simulation/Model/TwoFrameLinker.h +++ b/OpenSim/Simulation/Model/TwoFrameLinker.h @@ -23,7 +23,7 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -// INCLUDE +#include #include #include #include @@ -259,6 +259,25 @@ class TwoFrameLinker : public C { void addInPhysicalForcesFromInternal(const SimTK::State& state, SimTK::Vec6 f, SimTK::Vector_& spatialForces) const; + /** + * Produces system spatial forces given internal forces. + * + * Internal forces are expressed in the mobility basis as parameterized by the + * deflection between `frame1` and `frame2`, `dq`, and its time derivative, `dqdot`. + * + * This helper function is intended to be used as part of a `ForceProducer::implProduceForces` + * implementation. + * + * @param state the state used to evaluate forces + * @param f a `SimTK::Vec6` of forces in the basis of the deflection + * @param forceConsumer a `ForceConsumer` that shall recieve each evaluated force + */ + void producePhysicalForcesFromInternal( + const SimTK::State& state, + const SimTK::Vec6& f, + ForceConsumer& forceConsumer + ) const; + private: // create the frames property void constructProperties(); @@ -566,6 +585,34 @@ template void TwoFrameLinker::addInPhysicalForcesFromInternal( const SimTK::State& s, SimTK::Vec6 f, SimTK::Vector_& physicalForces) const +{ + // A `ForceConsumer` that applies body spatial vectors from `producePhysicalForcesFromInternal` + class Adaptor final : public ForceConsumer { + public: + explicit Adaptor(SimTK::Vector_& physicalForces) : + _physicalForces{&physicalForces} + {} + + private: + void implConsumeBodySpatialVec( + const SimTK::State& state, + const PhysicalFrame& body, + const SimTK::SpatialVec& spatialVec) final + { + (*_physicalForces)[body.getMobilizedBodyIndex()] += spatialVec; + } + + SimTK::Vector_* _physicalForces; + }; + + producePhysicalForcesFromInternal(s, f, Adaptor{physicalForces}); +} + +template +void TwoFrameLinker::producePhysicalForcesFromInternal( + const SimTK::State& s, + const SimTK::Vec6& f, + ForceConsumer& forceConsumer) const { SimTK::SpatialVec F_GF; SimTK::SpatialVec F_GM; @@ -586,9 +633,9 @@ void TwoFrameLinker::addInPhysicalForcesFromInternal( SimTK::SpatialVec F_GB2(F_GM[0] + p_B2M_G % F_GM[1], F_GM[1]); SimTK::SpatialVec F_GB1(F_GF[0] + p_B1F_G % F_GF[1], F_GF[1]); - // Apply (add-in) the body forces to the system set of body forces - physicalForces[frame2.getMobilizedBodyIndex()] += F_GB2; - physicalForces[frame1.getMobilizedBodyIndex()] += F_GB1; + // Produce the body forces as body spatial vectors. + forceConsumer.consumeBodySpatialVec(s, frame2, F_GB2); + forceConsumer.consumeBodySpatialVec(s, frame1, F_GB1); } diff --git a/OpenSim/Simulation/StatesDocument.cpp b/OpenSim/Simulation/StatesDocument.cpp new file mode 100644 index 0000000000..c709976f81 --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.cpp @@ -0,0 +1,769 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: StatesDocument.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2022-2024 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ +#include "StatesDocument.h" + +using namespace SimTK; +using namespace SimTK::Xml; +using namespace std; +using namespace OpenSim; +using std::cout; + +namespace OpenSim { + +// Anonymous namespace to ensure local linkage +namespace { + +//----------------------------------------------------------------------------- +// Local utility methods for use with class StatesDocument +//----------------------------------------------------------------------------- +struct SDocUtil { + + //_________________________________________________________________________ + template + static + void + appendVarElt(const string& path, const string& tag, const string& type, + const Array_& valArr, Element& parent, int precision) + { + // Create the variable element. + Element varElt(tag); + varElt.setAttributeValue("path", path); + varElt.setAttributeValue("type", type); + + // Append the variable element + varElt.setValueAs>(valArr, precision); + parent.appendNode(varElt); + } + //_________________________________________________________________________ + template + inline + static + void + getEltValue(const string& path, size_t expectedSize, + Element& varElt, Array_& vArr) + { + // Interpret the element value + varElt.getValueAs>(vArr); + + // Check the size + size_t n = vArr.size(); + SimTK_ASSERT3_ALWAYS(n == expectedSize, + "Found %d values in the element for %s, but there should be %d", + n, path.c_str(), expectedSize); + } + //_________________________________________________________________________ + template + inline + static + void + initializeStatesForStateVariable(Element& varElt, const Model& model, + const string& path, Array_ & traj) + { + // Interpret the element an array of type T + Array_ vArr; + getEltValue(path, traj.size(), varElt, vArr); + + // Set variable in the States trajectory + model.setStateVariableTrajectory(path, vArr, traj); + } + //_________________________________________________________________________ + template + inline + static + void + initializeStatesForDiscreteVariable(Element& varElt, const Model& model, + const string& path, Array_ & traj) + { + // Interpret the element an array of type T + Array_ vArr; + getEltValue(path, traj.size(), varElt, vArr); + + // Set variable in the States trajectory + model.setDiscreteVariableTrajectory(path, vArr, traj); + } + //_________________________________________________________________________ + template + inline + static + void + initializeStatesForModelingOption(Element& varElt, const Model& model, + const string& path, Array_ & traj) + { + // Interpret the Element value + Array_ vArr; + varElt.getValueAs>(vArr); + + // Check the sizes. + size_t n = vArr.size(); + SimTK_ASSERT2_ALWAYS(n == traj.size(), + "Found %d values. Should match nTime = %d values.", + n, traj.size()); + + // Set variable in the States trajectory + model.setModelingOptionTrajectory(path, vArr, traj); + } +}; + +} // End anonymous namespace +} // End OpenSim namespace + +// Note that the methods below are still in the OpenSim namespace. +// That namespace declaration is taken care of in the .h file. + +//----------------------------------------------------------------------------- +// Construction +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +StatesDocument:: +StatesDocument(const Model& model, const Array_& trajectory, + const String& note, int p) +{ + this->note = note; + this->precision = clamp(1, p, SimTK::LosslessNumDigitsReal); + formDoc(model, trajectory); +} +//_____________________________________________________________________________ +StatesDocument:: +StatesDocument(const Model& model, const vector& trajectory, + const String& note, int p) +{ + this->note = note; + this->precision = clamp(1, p, SimTK::LosslessNumDigitsReal); + + // Repackage the trajectory of states as a SimTK::Array_<>, which is + // the container type used by this class and also by the underlying + // trajectory-related methods in OpenSim::Component. + // + // The constructor below is shallow; it does not create copies of + // the contained State elements. The Array_<> refers directly to the + // contents of trajectory. Hence, the repackaging is quite inexpensive + // computationally. + // + // Unfortunately, this constructor does not have a const version, so + // the const modifier of trajectory has to be cast away. The vector is, + // however, safe from changes. Note that the method `formDoc()` only + // takes a const trajectory. + vector& trajectoryNonconst = const_cast&>(trajectory); + Array_ traj(trajectoryNonconst, SimTK::DontCopy()); + + formDoc(model, traj); +} + +//----------------------------------------------------------------------------- +// Serialize +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +serialize(const SimTK::String& filename) { + doc.writeToFile(filename); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formDoc(const Model& model, const Array_& traj) { + formRootElement(model, traj); + formNoteElement(model, traj); + formTimeElement(model, traj); + formContinuousElement(model, traj); + formDiscreteElement(model, traj); + formModelingElement(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formRootElement(const Model& model, const Array_& traj) { + // Set the tag of the root element and get an iterator to it. + doc.setRootTag("ostates"); + Element rootElt = doc.getRootElement(); + + // Insert a comment at the top level, just before the root node. + string info = "OpenSim States Document (Version: "; + info += std::to_string(model.getDocumentFileVersion()); + info += ")"; + Xml::Comment comment(info); + Xml::node_iterator root_it = doc.node_begin(Xml::ElementNode); + doc.insertTopLevelNodeBefore(root_it, comment); + + // Date and time + const std::time_t now = std::time(nullptr); + const char *localeName = "C"; + std::locale::global(std::locale(localeName)); + char buf[64]; + strftime(buf, sizeof buf, "%a %b %e %Y %H:%M:%S %Z", std::localtime(&now)); + + // Add attributes to the root node + rootElt.setAttributeValue("model", model.getName()); + rootElt.setAttributeValue("nTime", std::to_string(traj.size())); + rootElt.setAttributeValue("precision", std::to_string(precision)); + rootElt.setAttributeValue("date", buf); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formNoteElement(const Model& model, const Array_& traj) { + Element noteElt = Element("note"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(noteElt); + noteElt.setValue(note); +} +//_____________________________________________________________________________ +void +StatesDocument:: +formTimeElement(const Model& model, const Array_& traj) { + // Form time element. + Element timeElt = Element("time"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(timeElt); + + // Get time values from the StatesTrajectory + int n = (int)traj.size(); + SimTK::Array_ time(n); + for (int i = 0; i < n; ++i) { + time[i] = traj[i].getTime(); + } + + // Set the text value on the element + timeElt.setValueAs>(time, precision); +} +//_____________________________________________________________________________ +// Supported continuous variable type (October 2024): double +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +formContinuousElement(const Model& model, const Array_& traj) { + // Form continuous element. + Element contElt = Element("continuous"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(contElt); + + // Get a list of all state variables names from the model. + OpenSim::Array paths = model.getStateVariableNames(); + + // Loop over the names. + // Get the vector of values of each and append as a child element. + int n = paths.getSize(); + for (int i = 0; i < n; ++i) { + Array_ val; + model.getStateVariableTrajectory(paths[i], traj, val); + SDocUtil::appendVarElt(paths[i], "variable", "double", + val, contElt, precision); + } +} +//_____________________________________________________________________________ +// Supported discrete variable types (October 2024): +// bool, int, float, double, Vec2, Vec3, Vec4, Vec5, Vec6 +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim by adding the appropriate `else if` block below. +// +void +StatesDocument:: +formDiscreteElement(const Model& model, const Array_& traj) { + // Form discrete element. + Element discreteElt = Element("discrete"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(discreteElt); + + // Get a list of all discrete variable names from the model. + OpenSim::Array paths = model.getDiscreteVariableNames(); + + // Loop over the names. + // Get the vector of values for each and append as a child element. + int n = paths.getSize(); + for (int i = 0; i < n; ++i) { + // Get a single discrete variable so that its type can be discerned + const AbstractValue &v = + model.getDiscreteVariableAbstractValue(traj[0], paths[i]); + + // Append the vector according to type + if (SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "bool", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "int", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "float", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "double", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec2", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec3", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec4", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec5", + vArr, discreteElt, precision); + } + else if(SimTK::Value::isA(v)) { + Array_ vArr; + model.getDiscreteVariableTrajectory( + paths[i], traj, vArr); + SDocUtil::appendVarElt(paths[i], "variable", "Vec6", + vArr, discreteElt, precision); + } + else { + string msg = "Unrecognized type: " + v.getTypeName(); + SimTK_ASSERT(false, msg.c_str()); + } + } + +} +//_____________________________________________________________________________ +// Supported modeling option type (October 2024): int +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +formModelingElement(const Model& model, const Array_& traj) { + // Form continuous element. + Element modelingElt = Element("modeling"); + Element rootElt = doc.getRootElement(); + rootElt.appendNode(modelingElt); + + // Get a list of all modeling option names from the model. + OpenSim::Array paths = model.getModelingOptionNames(); + + // Loop over the names. + // Get the vector of values of each and append as a child element. + int n = paths.getSize(); + for (int i = 0; i < n; ++i) { + Array_ val; + model.getModelingOptionTrajectory(paths[i], traj, val); + SDocUtil::appendVarElt(paths[i], "option", "int", + val, modelingElt, precision); + } +} + + +//----------------------------------------------------------------------------- +// Deserialize +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +StatesDocument:: +deserialize(const Model& model, Array_& traj) { + checkDocConsistencyWithModel(model); + initializeNumberOfStateObjects(); + initializePrecision(); + initializeNote(); + prepareStatesTrajectory(model, traj); + initializeTime(traj); + initializeContinuousVariables(model, traj); + initializeDiscreteVariables(model, traj); + initializeModelingOptions(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +deserialize(const Model& model, vector& trajectory) { + checkDocConsistencyWithModel(model); + initializeNumberOfStateObjects(); + initializePrecision(); + initializeNote(); + + // Repackage the trajectory of states as a SimTK::Array_<>, which is + // the container type used by this class and also by the underlying + // trajectory-related methods in OpenSim::Component. + // The following constructor is shallow; it does not create copies of + // the contained State elements. The Array_<> refers directly to the + // contents of trajectory. Hence, 1) the repackaging is quite inexpensive + // computationally, and 2) when the contents of `traj` are changed, + // so are the contents of `trajectory`. + prepareStatesTrajectory(model, trajectory); + Array_ traj(trajectory, SimTK::DontCopy()); + + initializeTime(traj); + initializeContinuousVariables(model, traj); + initializeDiscreteVariables(model, traj); + initializeModelingOptions(model, traj); +} +//_____________________________________________________________________________ +void +StatesDocument:: +checkDocConsistencyWithModel(const Model& model) { + // At this point, only the model name is checked here. + // Many other aspects are checked for consistency than just the model + // name. Those are more easily checked as the doc is parsed. + + // Check that name of the model in the doc matches the name of the model'. + Element rootElt = doc.getRootElement(); + Attribute modelNameAttr = rootElt.getOptionalAttribute("model"); + SimTK_ASSERT1(modelNameAttr.isValid(), + "The 'model' attribute of the root element was not found in file %s.", + filename.c_str()); + const SimTK::String& modelName = modelNameAttr.getValue(); + if (modelName != model.getName()) { + SimTK::String msg = "The model name (" + modelName + ")"; + msg += " in states document " + filename + " does not match"; + msg += " the name of the OpenSim model (" + model.getName() + ")"; + msg += " for which the states are being deserialized."; + SimTK_ASSERT_ALWAYS(false, msg.c_str()); + } + +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializeNumberOfStateObjects() { + // The number of State objects should be the same as the number of time + // stamps. That is, nStateObjects = nTime. + Element rootElt = doc.getRootElement(); + Attribute nTimeAttr = rootElt.getOptionalAttribute("nTime"); + bool success = nTimeAttr.getValue().tryConvertTo(nStateObjects); + SimTK_ASSERT_ALWAYS(success, + "Unable to acquire nTime from root element."); + SimTK_ASSERT1_ALWAYS(nStateObjects > 0, + "Root element attribute numStateObjects=%d; should be > 0.", + nStateObjects); +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializePrecision() { + // Find the element + Element rootElt = doc.getRootElement(); + Attribute precisionAttr = rootElt.getOptionalAttribute("precision"); + int p; + bool success = precisionAttr.getValue().tryConvertTo(p); + SimTK_ASSERT_ALWAYS(success, + "Unable to acquire the precision from the root element."); + this->precision = clamp(1, p, SimTK::LosslessNumDigitsReal); +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializeNote() { + // Find the element + Element rootElt = doc.getRootElement(); + Array_ noteElts = rootElt.getAllElements("note"); + + // Check the number of note elements found. Should be 1. + if (noteElts.size() == 0) { + this->note = ""; + } + else if (noteElts.size() > 1) { + cout << "StatesDocument: More than 1 `note` element found; "; + cout << "using just the first one." << endl; + } + + // Get the value + this->note = noteElts[0].getValue(); +} +//_____________________________________________________________________________ +// Note that this method is overloaded to permit users the flexibility of +// using either SimTK::Array_<> or std::vector<> as the trajectory container. +void +StatesDocument:: +prepareStatesTrajectory(const Model& model, Array_& traj) { + // Create a local copy of the Model and get its default State. + Model localModel(model); + SimTK::State defaultState = localModel.initSystem(); + + // Append the needed number of state objects to the trajectory. + // A copy of the default state is made with each call of emplace_back(). + // These copies will be initialized during the rest of the deserialization + // process. + for (int i=0; i < nStateObjects; ++i) traj.emplace_back(defaultState); +} +//_____________________________________________________________________________ +// Note that this method is overloaded to permit users the flexibility of +// using either SimTK::Array_<> or std::vector<> as the trajectory container. +void +StatesDocument:: +prepareStatesTrajectory(const Model& model, vector& traj) { + // Create a local copy of the Model and get its default State. + Model localModel(model); + SimTK::State defaultState = localModel.initSystem(); + + // Append the needed number of state objects to the trajectory. + // A copy of the default state is made with each call of emplace_back(). + // These copies will be initialized during the rest of the deserialization + // process. + for (int i=0; i < nStateObjects; ++i) traj.emplace_back(defaultState); +} +//_____________________________________________________________________________ +void +StatesDocument:: +initializeTime(Array_& traj) { + // Find the element + Element rootElt = doc.getRootElement(); + Array_ timeElts = rootElt.getAllElements("time"); + + // Check the number of time elements found. Should be 1. + SimTK_ASSERT1_ALWAYS(timeElts.size() == 1, + "%d time elements found. Only 1 should be found.", timeElts.size()); + + // Get the values + Array_ timeArr; + timeElts[0].getValueAs>(timeArr); + + // Check the size of the time array. + size_t n = timeArr.size(); + SimTK_ASSERT2_ALWAYS(n == traj.size(), + "Found %d time values. Should match numStateObjects = %d", + n, traj.size()); + + // Initialize the State objects + for (size_t i = 0; i < n; ++i) traj[i].setTime(timeArr[i]); +} +//_____________________________________________________________________________ +// Supported continuous variable type (October 2024): double +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +initializeContinuousVariables(const Model& model, SimTK::Array_& traj) { + // Find the 'continuous' element + SimTK::String tag = "continuous"; + Element rootElt = doc.getRootElement(); + Array_ contElts = rootElt.getAllElements(tag); + SimTK_ASSERT1_ALWAYS(contElts.size() == 1, + "Found %d elements with tag 'continuous'. Should only be 1.", + contElts.size()); + + // Find all the child 'variable' elements + SimTK::String childTag = "variable"; + Array_ varElts = contElts[0].getAllElements(childTag); + + // Check that the number matches the number of continous variables. + // In OpenSim, a continuous variable is referred to as a StateVariable. + OpenSim::Array varNames = model.getStateVariableNames(); + int n = varElts.size(); + int m = varNames.size(); + SimTK_ASSERT2_ALWAYS(n == m, + "Found %d continuous variable elements. Should be %d.", n, m); + + // Loop over the variable elements + SimTK::Array_ varArr; + for (int i = 0; i < n; ++i) { + // type + Attribute typeAttr = varElts[i].getOptionalAttribute("type"); + const SimTK::String &type = typeAttr.getValue(); + + // path + Attribute pathAttr = varElts[i].getOptionalAttribute("path"); + const SimTK::String path = pathAttr.getValue(); + + // Switch based on the type. + // Type double is expected for continuous variable elements. + if (type == "double") { + SDocUtil::initializeStatesForStateVariable(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} +//_____________________________________________________________________________ +// Supported discrete variable types (October 2024): +// bool, int, float, double, Vec2, Vec3, Vec4, Vec5, Vec6 +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim by adding the appropriate `else if` block below. +// +void +StatesDocument:: +initializeDiscreteVariables(const Model& model, SimTK::Array_& traj) { + Element rootElt = doc.getRootElement(); + Array_ discElts = rootElt.getAllElements("discrete"); + SimTK_ASSERT1_ALWAYS(discElts.size() == 1, + "Found %d elements with tag 'discrete'. Only 1 should be found.", + discElts.size()); + + // Find all the child 'variable' elements + SimTK::String childTag = "variable"; + Array_ varElts = discElts[0].getAllElements(childTag); + + // Check that # children matches the number of discrete variables. + OpenSim::Array varNames = model.getDiscreteVariableNames(); + int n = varElts.size(); + int m = varNames.size(); + SimTK_ASSERT2_ALWAYS(n == m, + "Found %d discrete variable elements. Should be %d.", n, m); + + // Loop over the variable elements + for (int i = 0; i < n; ++i) { + // type + Attribute typeAttr = varElts[i].getOptionalAttribute("type"); + const SimTK::String &type = typeAttr.getValue(); + + // path + Attribute pathAttr = varElts[i].getOptionalAttribute("path"); + const SimTK::String path = pathAttr.getValue(); + + // Switch based on the type + // Append the vector according to type + if (type == "bool") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "int") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "float") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "double") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec2") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec3") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec4") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec5") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else if(type == "Vec6") { + SDocUtil::initializeStatesForDiscreteVariable(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} +//_____________________________________________________________________________ +// Supported continuous variable type (October 2024): int +// +// Any type that can be represented as a SimTK::Value can be supported +// in OpenSim. +// +// Refer to both formDiscreteElement() and initializeDiscreteVariables() for +// example code for handling variables of different types. +// +void +StatesDocument:: +initializeModelingOptions(const Model& model, SimTK::Array_& traj) { + // Find the element + Element rootElt = doc.getRootElement(); + Array_ modlElts = rootElt.getAllElements("modeling"); + SimTK_ASSERT1_ALWAYS(modlElts.size() == 1, + "%d modeling elements found. Only 1 should be found.", + modlElts.size()); + Element modlElt = modlElts[0]; + + // Find all the child 'variable' elements. + SimTK::String childTag = "option"; + Array_ varElts = modlElts[0].getAllElements(childTag); + + // Check that the number matches the number of continous variables. + // In OpenSim, a continuous variable is referred to as a StateVariable. + OpenSim::Array varNames = model.getModelingOptionNames(); + int n = varElts.size(); + int m = varNames.size(); + SimTK_ASSERT2_ALWAYS(n == m, + "Found %d modeling option elements. Should be %d.", n, m); + + // Loop over the modeling options + SimTK::Array_ varArr; + for (int i = 0; i < n; ++i) { + // type + Attribute typeAttr = varElts[i].getOptionalAttribute("type"); + const SimTK::String &type = typeAttr.getValue(); + + // path + Attribute pathAttr = varElts[i].getOptionalAttribute("path"); + const SimTK::String path = pathAttr.getValue(); + + // Switch based on the type. + // Type int is expected for modeling option elements. + if (type == "int") { + SDocUtil::initializeStatesForModelingOption(varElts[i], + model, path, traj); + } + else { + string msg = "Unrecognized type: " + type; + SimTK_ASSERT(false, msg.c_str()); + } + } +} diff --git a/OpenSim/Simulation/StatesDocument.h b/OpenSim/Simulation/StatesDocument.h new file mode 100644 index 0000000000..2032212799 --- /dev/null +++ b/OpenSim/Simulation/StatesDocument.h @@ -0,0 +1,664 @@ +#ifndef OPENSIM_STATES_DOCUMENT_H_ +#define OPENSIM_STATES_DOCUMENT_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: StatesDocument.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2023-2024 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +// INCLUDE +#include +#include "osimSimulationDLL.h" +#include + +namespace OpenSim { + +//============================================================================= +//============================================================================= +/** Class StatesDocument provides a means of writing (serializing) and +reading (deserializing) a complete time history of model states to and from +a file. This capability is key when analyzing model behavior, visualizing +simulation results, and conducting a variety of computationally demanding +tasks (e.g., fitting a model to experimental data, solving optimal +control problems, etc.). + +The states of an OpenSim::Model consist of all the independent variables that +change (or can change) during a simulation. At each time step during a +simulation, the underlying SimTK infrastructure captures the states in a +SimTK::State object. A state variable falls into one of the following +categories: + + 1) Continuous Variables (aka OpenSim::StateVariable%s) + 2) Discrete Variables + 3) Modeling Options + +Continuous Variables are governed by differential equations. They are +numerically integrated during a simulation based on the values of their +derivatives. Examples include joint coordinates, joint speeds, and muscle +activations. In OpenSim, because Continuous Variables are the most commonly +encountered kind of state, they are simply referred to as State Variables. All +concrete instances of Continuous Variables in OpenSim are derived from the +abstract class OpenSim::StateVariable. + +Discrete Variable are not governed by differential equations and so can change +discontinuously during a simulation. Examples can include inputs to a +simulation, like muscle excitations, coefficients of friction, and torque +motor voltages. Examples can also include outputs to a simulation, like points +of contact between colliding bodies and whether those bodies are experiencing +static or kinetic frictional conditions. Such output discrete variables are +updated at each time step during numerical integration. Unlike continuous +states, however, they are updated based on closed-form algebraic expressions +rather than based on their derivatives. In the underlying SimTK infrastructure, +an output discrete variable is implemented as a specialized kind of +discrete variable called an Auto-Update Discrete Variable. + +Modeling Options are flags, usually of type int, that are used to choose +between viable ways to model a SimTK::System or whether or not to apply a +constraint. Examples include a flag that specifies whether Euler angles or +quaternions are used to represent rotation or a flag that specifies whether a +particular joint coordinate is locked. When a Modeling Option is changed, +low-level aspects of the System must be reconstituted or, in SimTK +terminology, re-realized through SimTK::Stage::Model. + +Prior to the introduction of this class, only Continuous Variables (i.e., +OpenSim::StateVariable%s) were routinely and systematically serialized, +most commonly via the OpenSim::Manager as an OpenSim::Storage file +or via class OpenSim::StatesTrajectory as an OpenSim::TimeSeriesTable. +Discrete Variables and Modeling Options, if serialized, had to be stored in +separate files or handled as OpenSim::Property objects. In addition, prior to +this class, all Discrete Variables in OpenSim were assumed to be type double, +which is not a requirement of the underlying SimTK infrastructure. + +With the introduction of this class, all state variables {i.e., Continuous +Variables (OpenSim::StateVariable%s), Discrete Variables, and Modeling Options} +can be serialized in a single file, which by convention has the `.ostates` +file name exention. In addition, a variety of types (e.g., bool, int, double, +Vec3, Vec4, etc.) are supported for Discrete Variables. Continuous States are +still assumed to be type double, and Modeling Options are still assumed to be +type `int`. Note, however, that the `.ostates` file format has the +flexibility to relax these assumptions and include other types if needed. + +@note A point of clarification about Data Cache Variables... +By definition, state variables are independent. That is, the value of one +cannot be determined from the values of others. If a quantity of interest can +be computed from values of state variables, particularly if that quantity is +needed frequently, that quantity is often formalized as a Data Cache Variable. +The value of a Data Cach Variable is computed at each time step of a simulation +and stored in the SimTK::State. However, because a Data Cache Variable can +always be computed from the Continuous Variables, Discrete Variables, and +Modeling Options, they are not serialized. + + SimTK::State Contents | Serialized in `.ostates`? + ------------------------ | ----------------------- + Continuous Variables | yes + Discrete Variables | yes + Modeling Options | yes + Data Cache Variables | no + + +----------------- +Design Notes +----------------- + +### Dependencies +Most operations in class StatesDocument rely on underlying SimTK classes, +most notably SimTK::String, SimTK::Array, SimTK::State, and SimTK::Xml. + +StatesDocument has just one key OpenSim dependency: OpenSim::Model. +OpenSim::Model brings with it all the methods it inherits from class +OpenSim::Component, which are essential for getting and setting state +information in OpenSim. StatesDocument does not know about classes like +OpenSim::Storage, OpenSim::TimeSeriesTable, OpenSim::StatesTrajectory, or +OpenSim::Manager. + +Exchanges of state information between class StatesDocument and the rest of +OpenSim are accomplished via objects of type SimTK::Array_, or +alternatively std::vector, which are informally referred to as +state trajectories (see directly below). + +### Trajectories +In many methods of this class, as well as in related classes, you will +encounter the term 'trajectory'. In these contexts, the term connotes a +time-ordered sequence, or a time-history, of values. + +An array of knee angles (-10.0, -2.3, 4.5, 6.2, 7.1) would be termed a knee +angle trajectory if those knee angles were recorded sequentially during a +simulation. Similarly, an array of SimTK::State objects, if time ordered, +would be called a states trajectory. + +Because of the flexibility and computational speed of the SimTK::Array_ +container class, you will often see trajectories passed in argument lists as +SimTK::Array_%s. SimTK::Array_ might represent the trajectory of a +knee angle. SimTK::Array_ might represent the trajectory of the +center of pressure between a foot and the floor during a walking motion. +SimTK::Array_ is used to capture the full trajectory of states +(continuous variables, discrete variables, and modeling options) recorded +during a simulation. + +@Note SimTK::Array_ is preferred over std::vector +for reasons of performance, binary compatibility with Simbody libraries, and +consistency with Simbody's underlying code base. For a variety of common +operations, like indexing through an array, SimTK::Array_ is about +twice as fast as std::vector_ on Windows systems. Such speed differences +may not be as large on Mac or Ubuntu systems, but it is safe to assume that +SimTK::Array_ will be just as fast or have a speed advantage. + +Th `StatesDocument` class relies heavily on a few trjectory-centric methods +available in the OpenSim::Component class. A few examples follow. + +``` + template + Component::getDiscreteVariableTrajectory( + const std::string& path, + const SimTK::Array_& input, + SimTK::Array_& output) const +``` +A call to the above method first finds a Discrete Variable in the component +hierarchy based on the specifed path (`path`). Then, from the input states +trajectory (`input`), the method extracts the values of the specified +Discrete Variable and returns its trajectory as the output (`output`). +Notice that the type of the Discrete Variable can be specified by the caller +(i.e., T = int, double, Vec3, Vec4, etc.). + +``` + template + void setDiscreteVariableTrajectory( + const std::string& path, + const SimTK::Array_& input, + SimTK::Array_& output) const +``` +On the other hand, based on the input trajectory of a specified Discrete +Variable (`input`), a call to the above method sets the appropriate +element in each of the SimTK::State objects held in the states trajectory +(`output`). Notice again that the type T of the Discrete Variable can be +specified by the caller. + +### Complete and Constant XML Document upon Construction +Upon construction, a StatesDocument instance always contains a complete +internal XML document that represents a complete serialization of a specific +model's state trajectory. Moreover, that internal XML document cannot be +altered after construction! + +If a model is changed (e.g., a muscle or contact element is added) or +a change has occurred in its state trajectory, the intended way to generate +an XML document that reflects those changes is to construct a new +StatesDocument instance. Constructing a new instance is the most reliable +approach for ensuring an accurate serialization. This approach also greatly +simplifies the implementation of the StatesDocument class, as methods for +selectively editing aspects of the internal XML document are consequently +unnecessary. + +### Output Precision +The precision with which numbers are serialized to a `.ostates` file can be +specified at the time of construction. The `precision` parameter specifies +the maximum number of significant digits used to represent numbers. If a +number can be represented without data loss with fewer digits, fewer digits +are used. In other words, trailing zeros are not written to file, thus +reducing file size. For example, if `precision` = 5, the number +1.50000000000000000000 would be represented in a `.ostates` file +as '1.5'; however, Ï€ would be represented as '3.1415'. + +By default, the `precision` parameter of a `StatesDocument` is set to the +constant `SimTK::LosslessNumDigitsReal`, which results in lossless +serialization. When `precision` = `SimTK::LosslessNumDigitsReal`, the +`SimTK::State` can be serialized and deserialized repeatedly without loss +of information. `SimTK::LosslessNumDigitsReal` is platform dependent but +typically has a value of about `20`. In applications where exact values of the +states are needed, lossless precision should be used. In applications where +exact values of the states are not needed, a smaller number of digits can be +used (e.g., `precsion = 6`) as a means of reducing the size of a `.ostates` +file or simplifying some types of post analysis (e.g., plotting where the extra +significant figures would go unnoticed). + + +------------------- +.ostate File Format +------------------- +XML is used as the organizing framework for `.ostates` files +(see SimTK::Xml), allowing them to be viewed and edited with a text editor. +Internet browsers can be also be used to view a `.ostate` file but may +require a `.xml` file extension to be added to the file name for the +XML format to be recognized. + +### Sample `.ostates` File +``` + + + + + + (0,7.14, ...) + (0,7.81, ...) + ... + + + (~[2.1,-1.1,0],~[1.82,-1.1,0], ...) + (0.5,0.5, ...) + (0.7,0.7, ...) + (1,1, ...) + ... + + + + + ... + + +``` + +### Deserialization Requirements +Successful deserialization of a .ostates file and full initialization of a +states trajectory for an OpenSim::Model requires the following: + + 1) The name of the `OpenSim::Model` must match the value of the + `model` attribute of the top-level `ostates` element. + + 2) The number of continuous variables, discrete variables, and modeling + options in the .ostates file must match the corresponding numbers in the + OpenSim::Model. + + 3) The number of values recorded for each `variable` and each + `option` in the `.ostates` file must be equal to the value of the + `nTime` attribute of the top-level `ostates` element. + + 4) All `variable` and `option` paths must be found in the model + OpenSim::Component heirarchy. + + 5) The type must be supported. As of September 2024, the following types + are supported: + + SimTK::State Category | Supported Type(s) + ------------------------ | ----------------------- + Continuous Variables | double + | + Discrete Variables | bool, int, float, double, + | Vec2, Vec3, Vec4, Vec5, Vec6 + | + Modeling Options | int + + +-------------------------- +Using Class StatesDocument +-------------------------- +Below are some code snippets that show how the StatesDocument class can be +used. Example 1 shows how to obtain a states trajectory from a simulation and +then serialize those states to file. Example 2 shows how to follow up and +deserialize those same states and use them to accomplish a few basic things. + +### Example 1: Serializing Simulated States +``` + // --------------- + // Build the Model + // --------------- + // Building a model can be done in many ways. The most common approach is + // to construct a model from an OpenSim model file. Here, an empty model is + // constructed with place holders for components that are typically added. + OpenSim::Model model(); + model.setGravity( Vec3(0.0,-9.8,0.0) ); + model.setName("BouncingBlock"); + // Add bodies... + // Add joints... + // Add actuators & contact elements... + + // ------------------------------- + // Add a StatesTrajectory Reporter + // ------------------------------- + // The reporter records the SimTK::State in an std::vector<> at a + // specified time interval. + OpenSim::StatesTrajectoryReporter* reporter = + new StatesTrajectoryReporter(); + reporter->setName("states_reporter"); + double interval = 0.01; + reporter->set_report_time_interval(interval); + model->addComponent(reporter); + + // ----------------------------------------- + // Build the System and Initialize the State + // ----------------------------------------- + model.buildSystem(); + SimTK::State& state = model.initializeState(); + + // --------- + // Integrate + // --------- + Manager manager(*model); + manager.getIntegrator().setMaximumStepSize(0.01); + manager.setIntegratorAccuracy(1.0e-5); + double ti = 0.0; + double tf = 5.0; + state.setTime(ti); + manager.initialize(state); + state = manager.integrate(tf); + + // ----------------------- + // Create a StatesDocument + // ----------------------- + // The reporter that was added to the system collects the states in an + // OpenSim::StatesTrajectory object. Underneath the covers, the states are + // accumulated in a private array of state objects (i.e., vector). + // The StatesTrajectory class knows how to export a StatesDocument based + // on those states. This "export" functionality is what is used below. + const StatesTrajectory& trajectory = reporter->getStates(); + StatesDocument doc = trajectory.exportToStatesDocument(model); + + // Alternatively, a read-only reference to the underlying state array + // can be obtained from the reporter and used to construct a + // StatesDocument directly: + const std::vector& traj = reporter->getVectorOfStateObjects(); + StatesDocument doc(model, traj); + + // ---------------------------- + // Serialize the States to File + // ---------------------------- + // The file name (see below), can be any string supported by the file + // system. The recommended convention is for the file name to carry the + // suffix ".ostates". Below, the suffix ".ostates" is simply added to + // the name of the model, and the document is saved to the current working + // directory. The file name can also incorporate a valid system path (e.g., + // "C:/Users/smith/Documents/Work/BouncingBlock.ostates"). + SimTK::String statesFileName = model.getName() + ".ostates"; + doc.serializeToFile(statesFileName); + + // ---------------------- + // Save the Model to File + // ---------------------- + SimTK::String modelFileName = model.getName() + ".osim"; + model->print(modelFileName); + +``` + +### Example 2: Deserializing States +``` + // ----------------------------- + // Construct the Model from File + // ----------------------------- + SimTK::String name = "BouncingBlock"; + SimTK::String modelFileName = name + ".osim"; + OpenSim::Model model(modelFileName); + model.buildSystem(); + SimTK::State& initState = model->initializeState(); + + // ----------------------------------------------- + // Construct the StatesDocument Instance from File + // ----------------------------------------------- + SimTK::String statesFileName = name + ".ostates"; + StatesDocument doc(statesFileName); + + // ---------------------- + // Deserialize the States + // ---------------------- + // Note that model and document must be entirely consistent with each + // other for the deserialization to be successful. + // See StatesDocument::deserialize() for details. + SimTK::Array_ traj; // Or, std::vector traj; + doc.deserialize(model, traj); + + // Below are some things that can be done once a deserialized state + // trajectory has been obtained. + + // --------------------------------------------------- + // Iterate through the State Trajectory Getting Values + // --------------------------------------------------- + std::string path; + const SimTK::State* iter; + for(iter = traj.cbegin(); iter!=traj.cend(); ++iter) { + + // Get time + double t = iter->getTime(); + + // Get the value of a continuous state + path = "/jointset/free/free_coord_0/value"; + double x = model.getStateVariableValue(*iter, path); + + // Get the value of a discrete state of type double + path = "/forceset/EC0/sliding"; + double sliding = model.getDiscreteVariableValue(*iter, path); + + // Get the value of a discrete state of type Vec3 + path = "/forceset/EC0/anchor" + const SimTK::AbstractValue& valAbs = + model.getDiscreteVariableAbstractValue(*iter, path); + SimTK::Value valVec3 = SimTK::Value::downcast( valAbs ); + Vec3 anchor = valVec3.get(); + + // Get the value of a modeling option + path = "/jointset/free/free_coord_0/is_clamped"; + int clamped = model.getModelingOption(*iter, path); + + // Access the value of a data cache variable. Note that this will + // require state realization at the appropriate stage. + system.realize(*iter, SimTK::Stage::Dynamics); + Vec3 force = forces.getContactElement(0)->getForce(); + } + + // ---------------------------------------------------- + // Extract a Complete Trajectory for a Particular State + // ---------------------------------------------------- + // Continuous (double) + path = "/jointset/free/free_coord_0/value"; + SimTK::Array_ xTraj; + model.getStateVariableTrajectory(path, traj, xTraj); + + // Discrete (Vec3) + path = "/forceset/EC0/anchor"; + SimTK::Array_ anchorTraj; + model.getDiscreteVariableTrajectory(path, traj, anchorTraj); + + // Modeling (int) + path = "/jointset/free/free_coord_0/is_clamped"; + SimTK::Array_ clampedTraj; + model.getModelingOptionTrajectory(path, traj, clampedTraj); + + // ---------------------- + // Form a TimeSeriesTable + // ---------------------- + // Note that the table will only include the continuous states. + // This might be done for plotting, post analysis, etc. + StatesTrajectory trajectory(model, doc); + OpenSim::TimesSeriesTable table = traj.exportToTable(model); + +``` + +### A Final Note +Because Storage files (*.sto) and TimeSeriesTable files (*.tst) typically +capture only the continuous states of a system, using these files as the basis +for deserialization runs the risk of leaving discrete variables and modeling +options in the SimTK::State uninitialized. In such an approach, additional +steps may be needed to properly initialize all variables in the SimTK::State +(e.g., by relying on OpenSim::Properties and/or on supplemental input files). + +In contrast, the StatesDocument class can be relied upon to yield a complete +serialization and deserialization of the SimTK::State. If the StatesDocument +class is used to serialize and then deserialize a state trajectory that was +recorded during a simulation, all state variables in the State (continuous, +discrete, and modeling) will be saved to a single file during serizaliztion +and initialized upon deserialization of the document. + +@authors F. C. Anderson **/ +class OSIMSIMULATION_API StatesDocument { + +public: + //------------------------------------------------------------------------- + // Construction + //------------------------------------------------------------------------- + /** The default constructor serves no purpose other than satisfying a + compiler demand. */ + StatesDocument() { } + + /** Construct a StatesDocument instance from an XML file in preparation + for deserialzing the states into a states trajectory. Once constructed, + the document is not designed to be modified; it is a fixed snapshot of the + states stored by the file at the time of construction. If the XML file + changes, the intended mechanism for obtaining a document that is + consistent with the modifed XML file is simply to construct a new document. + By convention (and not requirement), a StatesDocument filename has + ".ostates" as its suffix. To deserialize the states, call + StatesDocument::deserialize() on the constructed document. Note that the + validity of the XML file is not tested until StatesDocument::deserialize() + is called. + + @param filename The name of the file, which may be prepended by the system + path at which the file resides (e.g., "C:/Documents/block.ostates"). */ + StatesDocument(const SimTK::String& filename) : filename(filename) { + doc.readFromFile(filename); + initializeNote(); + initializePrecision(); + } + + /** Construct a StatesDocument instance from a states trajectory in + preparation for serializing the trajectory to file. Once constructed, the + document is not designed to be modified; it is a fixed snapshot of the + states trajectory at the time of construction. The intended mechanism for + obtaining a document that is consistent with a modified or new states + trajectory is simply to construct a new document. To serialize the + constructed document to file, call StatesDocument::serialize(). + + @param model The OpenSim::Model to which the states belong. + @param trajectory An array containing the time-ordered sequence of + SimTK::State objects. + @param note Annotation note for this states document. By default, the note + is an empty string. + @param precision The number of significant figures with which numerical + values are converted to strings. The default value is + SimTK:LosslessNumDigitsReal (about 20), which allows for lossless + reproduction of state. */ + StatesDocument(const OpenSim::Model& model, + const SimTK::Array_& trajectory, + const SimTK::String& note = "", + int precision = SimTK::LosslessNumDigitsReal); + + /** Construct a StatesDocument instance from a states trajectory in + preparation for serializing the trajectory to file. Once constructed, the + document is not designed to be modified; it is a fixed snapshot of the + states trajectory at the time of construction. The intended mechanism for + obtaining a document that is consistent with a modified or new states + trajectory is simply to construct a new document. To serialize the + constructed document to file, call StatesDocument::serialize(). + + @param model The OpenSim::Model to which the states belong. + @param trajectory An array containing the time-ordered sequence of + SimTK::State objects. + @param note Annotation note for this states document. By default, the note + is an empty string. + @param precision The number of significant figures with which numerical + values are converted to strings. The default value is + SimTK:LosslessNumDigitsReal (about 20), which allows for lossless + reproduction of state. */ + StatesDocument(const OpenSim::Model& model, + const std::vector& trajectory, + const SimTK::String& note = "", + int precision = SimTK::LosslessNumDigitsReal); + + //------------------------------------------------------------------------- + // Accessors + //------------------------------------------------------------------------- + /** Get the annotation note for this states document. */ + const SimTK::String& getNote() const { return note; } + /** Get the precision for this states document. */ + int getPrecision() const { return precision; } + + //------------------------------------------------------------------------- + // Serialization + //------------------------------------------------------------------------- + /** Serialize the document to file. By convention (and not requirement), + a StatesDocument filename has ".ostates" as its suffix. + + @param filename The name of the file, which may include the file system + path at which to write the file (e.g., "C:/Documents/block.ostates"). */ + void serialize(const SimTK::String& filename); + + //------------------------------------------------------------------------- + // Deserialization + //------------------------------------------------------------------------- + /** Deserialize the states held by this document into a states trajectory. + If deserialization fails, an exception describing the reason for the + failure is thrown. For details, see the section called "Deserialization + Requirements" in the introductory documentation for this class. + @note This method is overloaded to allow users the flexibility to use + either `SimTK::Array_<>` or `std::vector` as the trajectory container. + + @param model The OpenSim::Model with which the states are to be associated. + @param trajectory The array into which the time-ordered sequence of + SimTK::State objects will be deserialized. + @throws SimTK::Exception */ + void deserialize(const OpenSim::Model& model, + SimTK::Array_& trajectory); + + /** Deserialize the states held by this document into a states trajectory. + If deserialization fails, an exception describing the reason for the + failure is thrown. For details, see the section called "Deserialization + Requirements" in the introductory documentation for this class. + @note This method is overloaded to allow users the flexibility to use + either `SimTK::Array_<>` or `std::vector` as the trajectory container. + + @param model The OpenSim::Model with which the states are to be associated. + @param trajectory The array into which the time-ordered sequence of + SimTK::State objects will be deserialized. + @throws SimTK::Exception */ + void deserialize(const OpenSim::Model& model, + std::vector& trajectory); + +protected: + // Serialization Helpers. + void formDoc(const Model& model, + const SimTK::Array_& traj); + void formRootElement(const Model& model, + const SimTK::Array_& traj); + void formNoteElement(const Model& model, + const SimTK::Array_& traj); + void formTimeElement(const Model& model, + const SimTK::Array_& traj); + void formContinuousElement(const Model& model, + const SimTK::Array_& traj); + void formDiscreteElement(const Model& model, + const SimTK::Array_& traj); + void formModelingElement(const Model& model, + const SimTK::Array_& traj); + + // Deserialization Helpers. + void checkDocConsistencyWithModel(const Model& model); + void initializeNumberOfStateObjects(); + void prepareStatesTrajectory(const Model& model, + SimTK::Array_ &traj); + void prepareStatesTrajectory(const Model& model, + std::vector &traj); + void initializeNote(); + void initializePrecision(); + void initializeTime(SimTK::Array_ &traj); + void initializeContinuousVariables(const Model& model, + SimTK::Array_ &traj); + void initializeDiscreteVariables(const Model& model, + SimTK::Array_ &traj); + void initializeModelingOptions(const Model& model, + SimTK::Array_ &traj); + +private: + // Member Variables + int precision{SimTK::LosslessNumDigitsReal}; + int nStateObjects{0}; + SimTK::Xml::Document doc; + SimTK::String filename{""}; + SimTK::String note{""}; + +}; // END of class StatesDocument + +} // end of namespace OpenSim + +#endif // OPENSIM_STATES_DOCUMENT_H_ diff --git a/OpenSim/Simulation/StatesTrajectory.h b/OpenSim/Simulation/StatesTrajectory.h index 8a4b265dcf..133aecab9d 100644 --- a/OpenSim/Simulation/StatesTrajectory.h +++ b/OpenSim/Simulation/StatesTrajectory.h @@ -28,6 +28,7 @@ #include #include #include +#include #include "osimSimulationDLL.h" @@ -47,7 +48,7 @@ class Model; // TODO See the bottom of this file for a class description to use once the // OSTATES file format is implemented. // -/** +/** * \section StatesTrajectory * This class holds a sequence of SimTK::State%s. You can obtain a * StatesTrajectory during a simulation via the StatesTrajectoryReporter. You @@ -75,7 +76,7 @@ class Model; * Python and MATLAB do not enforce constness and thus allow modifying the * trajectory. * - * \subsection st_using_model Using with an OpenSim:: Model + * \subsection st_using_model Using with an OpenSim:: Model * A StatesTrajectory is not very useful on its own, since neither the * trajectory nor the contained states know how the Component%s name the state * variables they create. You probably want to use the trajectory with an @@ -151,7 +152,7 @@ class OSIMSIMULATION_API StatesTrajectory { /// @{ /** Get a const reference to the state at a given index in the trajectory. * Here's an example of getting a state variable value from the first state - * in the trajectory: + * in the trajectory: * @code{.cpp} * Model model("subject01.osim"); * const StatesTrajectory states = getStatesTrajectorySomehow(); @@ -172,20 +173,20 @@ class OSIMSIMULATION_API StatesTrajectory { try { return m_states.at(index); } catch (const std::out_of_range&) { - OPENSIM_THROW(IndexOutOfRange, index, 0, + OPENSIM_THROW(IndexOutOfRange, index, 0, static_cast(m_states.size() - 1)); } } /** Get a const reference to the first state in the trajectory. */ - const SimTK::State& front() const { + const SimTK::State& front() const { return m_states.front(); } /** Get a const reference to the last state in the trajectory. */ - const SimTK::State& back() const { + const SimTK::State& back() const { return m_states.back(); } /// @} - + /** Iterator type that does not allow modifying the trajectory. * Most users do not need to understand what this is. */ typedef std::vector::const_iterator const_iterator; @@ -289,6 +290,56 @@ class OSIMSIMULATION_API StatesTrajectory { TimeSeriesTable exportToTable(const Model& model, const std::vector& stateVars = {}) const; + /** Export a complete trajectory of states (i.e., one that includes + * all continuous, discrete, and modeling states) to an + * OpenSim::StatesDocument. That StatesDocument instance can then be + * used to serialize the states to an OSTATES file or document string by + * calling `StatesDocument::serialize()`. + * + * Once the states have been serialized, they can be deserialized by + * constructing a new StatesDocument by calling + * ``` + * StatesDocument(const SimTK::String& filename) + * ``` + * and then calling: + * ``` + * StatesDocument::deserialize(const OpenSim::Model& model, + * std::vector& trajectory) + * ``` + * + * The .ostates format is plain-text XML (see SimTK::Xml) with a + * specifiable precision between 1 and 20 significant figures. A precision + * of 20 digits results in losselss de/serialization. + * + * A note of CAUTION: + * Using either + * + * StatesTrajectory StatesTrajectory::createFromStatesStorage() or + * StatesTrajectory StatesTrajectory::createFromStatesTable() + * + * to construct a StatesTrajectory instance will likely leave discrete + * states (i.e., OpenSim::DiscreteVariable%s) and modeling states + * (i.e., OpenSim::ModelingOptions%s) uninitialized. The reason is that + * Storage and TimeSeriesTable objects include only the continuous states + * (i.e., OpenSim::StateVariable%s). + * + * Thus, when relying on serialization and deserialization to reproduce a + * complete StatesTrajectory, a StatesDocument is the preferred means as + * it will include continuous, discrete, and modeling states. + */ + OpenSim::StatesDocument + exportToStatesDocument(const OpenSim::Model& model, + const SimTK::String& note = "", + int precision = SimTK::LosslessNumDigitsReal) const + { + return OpenSim::StatesDocument(model, m_states, note, precision); + } + + /** Get a read-only reference to the underlying state array. */ + const std::vector& getStateArray() const { + return m_states; + } + private: std::vector m_states; @@ -337,11 +388,11 @@ class OSIMSIMULATION_API StatesTrajectory { msg += " " + missingStates[i] + "\n"; } msg += " " + missingStates.back(); - + addMessage(msg); } }; - + /** Thrown when trying to create a StatesTrajectory from states data, and * the data contains columns that do not correspond to continuous state * variables. */ @@ -360,7 +411,7 @@ class OSIMSIMULATION_API StatesTrajectory { msg += " " + extraStates[i] + "\n"; } msg += " " + extraStates.back(); - + addMessage(msg); } }; @@ -519,7 +570,7 @@ class OSIMSIMULATION_API StatesTrajectory { * * A SimTK::State object contains many different types of data, but only some * are saved into the OSTATES file: - * + * * type of data | saved in OSTATES? * ---------------------------- | ----------------- * (continuous) state variables | yes diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.cpp b/OpenSim/Simulation/StatesTrajectoryReporter.cpp index 4e0939e41a..e738544231 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.cpp +++ b/OpenSim/Simulation/StatesTrajectoryReporter.cpp @@ -34,6 +34,11 @@ const StatesTrajectory& StatesTrajectoryReporter::getStates() const { return m_states; } +const std::vector& +StatesTrajectoryReporter::getVectorOfStateObjects() const { + return m_states.getStateArray(); +} + /* TODO we have to discuss if the trajectory should be cleared. void StatesTrajectoryReporter::extendRealizeInstance(const SimTK::State& state) const { diff --git a/OpenSim/Simulation/StatesTrajectoryReporter.h b/OpenSim/Simulation/StatesTrajectoryReporter.h index 3ef28fb640..a17e750413 100644 --- a/OpenSim/Simulation/StatesTrajectoryReporter.h +++ b/OpenSim/Simulation/StatesTrajectoryReporter.h @@ -41,9 +41,11 @@ class OSIMSIMULATION_API StatesTrajectoryReporter : public AbstractReporter { OpenSim_DECLARE_CONCRETE_OBJECT(StatesTrajectoryReporter, AbstractReporter); public: - /** Access the accumulated states. */ - const StatesTrajectory& getStates() const; - /** Clear the accumulated states. */ + /** Obtain the accumulated states as a StatesTrajectory object. */ + const StatesTrajectory& getStates() const; + /** Obtain the accumulated states as a low-level array of states. */ + const std::vector& getVectorOfStateObjects() const; + /** Clear the accumulated states. */ void clear(); protected: diff --git a/OpenSim/Simulation/TableProcessor.h b/OpenSim/Simulation/TableProcessor.h index 21aa4c4f1d..4542f2f2cc 100644 --- a/OpenSim/Simulation/TableProcessor.h +++ b/OpenSim/Simulation/TableProcessor.h @@ -258,7 +258,7 @@ class OSIMSIMULATION_API TabOpAppendCoupledCoordinateValues }; /** Invoke SimulationUtilities::appendCoordinateValueDerivativesAsSpeeds() on -the table */ +the table. */ class OSIMSIMULATION_API TabOpAppendCoordinateValueDerivativesAsSpeeds : public TableOperator { OpenSim_DECLARE_CONCRETE_OBJECT( diff --git a/OpenSim/Simulation/Test/testForceProducer.cpp b/OpenSim/Simulation/Test/testForceProducer.cpp new file mode 100644 index 0000000000..0c07915d99 --- /dev/null +++ b/OpenSim/Simulation/Test/testForceProducer.cpp @@ -0,0 +1,446 @@ +/* -------------------------------------------------------------------------- * +* OpenSim: testForceProducer.h * +* -------------------------------------------------------------------------- * +* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * +* See http://opensim.stanford.edu and the NOTICE file for more information. * +* OpenSim is developed at Stanford University and supported by the US * +* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * +* through the Warrior Web program. * +* * +* Copyright (c) 2005-2024 Stanford University and the Authors * +* Author(s): Adam Kewley * +* * +* Licensed under the Apache License, Version 2.0 (the "License"); you may * +* not use this file except in compliance with the License. You may obtain a * +* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * +* * +* Unless required by applicable law or agreed to in writing, software * +* distributed under the License is distributed on an "AS IS" BASIS, * +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * +* See the License for the specific language governing permissions and * +* limitations under the License. * +* -------------------------------------------------------------------------- */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace OpenSim; + +namespace +{ + // A trivial example of a `ForceProducer` for the purposes of testing. + class ExampleForceProducer final : public ForceProducer { + OpenSim_DECLARE_CONCRETE_OBJECT(ExampleForceProducer, ForceProducer); + public: + ExampleForceProducer() = default; + + ExampleForceProducer( + size_t numGeneralizedForcesToProduce, + size_t numBodySpatialVectorsToProduce, + size_t numPointForcesToProduce) : + + _numGeneralizedForcesToProduce{numGeneralizedForcesToProduce}, + _numBodySpatialVectorsToProduce{numBodySpatialVectorsToProduce}, + _numPointForcesToProduce{numPointForcesToProduce} + {} + private: + void implProduceForces( + const SimTK::State& state, + ForceConsumer& consumer) const final + { + for (size_t i = 0; i < _numGeneralizedForcesToProduce; ++i) { + consumer.consumeGeneralizedForce(state, _dummyCoordinate, static_cast(i)); + } + for (size_t i = 0; i < _numBodySpatialVectorsToProduce; ++i) { + const SimTK::Vec3 torque{static_cast(i)}; + const SimTK::Vec3 force{static_cast(i)}; + consumer.consumeBodySpatialVec(state, _dummyBody, SimTK::SpatialVec{torque, force}); + } + for (size_t i = 0; i < _numPointForcesToProduce; ++i) { + const SimTK::Vec3 point{static_cast(i)}; + const SimTK::Vec3 force{static_cast(i)}; + consumer.consumePointForce(state, _dummyBody, point, force); + } + } + + OpenSim::Coordinate _dummyCoordinate; + OpenSim::Body _dummyBody; + size_t _numGeneralizedForcesToProduce = 0; + size_t _numBodySpatialVectorsToProduce = 0; + size_t _numPointForcesToProduce = 0; + }; + + // A trivial example of a `ForceConsumer` for the purposes of testing. + class ExampleForceConsumer final : public ForceConsumer { + public: + size_t getNumGeneralizedForcesConsumed() const { return _numGeneralizedForcesConsumed; } + size_t getNumBodySpatialVectorsConsumed() const { return _numBodySpatialVectorsConsumed; } + size_t getNumPointForcesConsumed() const { return _numPointForcesConsumed; } + + private: + + void implConsumeGeneralizedForce(const SimTK::State&, const Coordinate&, double) final + { + ++_numGeneralizedForcesConsumed; + } + + void implConsumeBodySpatialVec(const SimTK::State&, const PhysicalFrame&, const SimTK::SpatialVec&) final + { + ++_numBodySpatialVectorsConsumed; + } + + void implConsumePointForce(const SimTK::State&, const PhysicalFrame&, const SimTK::Vec3&, const SimTK::Vec3&) final + { + ++_numPointForcesConsumed; + } + + size_t _numGeneralizedForcesConsumed = 0; + size_t _numBodySpatialVectorsConsumed = 0; + size_t _numPointForcesConsumed = 0; + }; + + // A trivial implementation of a `ForceProducer` that can be added to a `Model` + // for the purposes of comparing side-effects to the `Force` API (see `MockForce` + // below). + class MockForceProducer final : public ForceProducer { + OpenSim_DECLARE_CONCRETE_OBJECT(MockForceProducer, ForceProducer); + public: + OpenSim_DECLARE_SOCKET(force_target, OpenSim::PhysicalFrame, "the physical frame that forces should be produced for"); + OpenSim_DECLARE_SOCKET(generalized_force_target, OpenSim::Coordinate, "the coordinate that generalized forces should be produced for"); + + explicit MockForceProducer( + const OpenSim::PhysicalFrame& forceTarget, + const OpenSim::Coordinate& generalizedForceTarget) + { + connectSocket_force_target(forceTarget); + connectSocket_generalized_force_target(generalizedForceTarget); + } + + private: + void implProduceForces(const SimTK::State& state, ForceConsumer& consumer) const final + { + // Note: this should be logically equivalent to `MockForce::computeForce` (below), so + // that the `ForceProducer`'s default `computeForce` implementations can be compared + // in an end-to-end way. + + consumer.consumeBodySpatialVec( + state, + getConnectee("force_target"), + SimTK::SpatialVec{SimTK::Vec3{1.0}, SimTK::Vec3{2.0}} + ); + + consumer.consumeBodySpatialVec( + state, + getConnectee("force_target"), + SimTK::SpatialVec{SimTK::Vec3{-0.5}, SimTK::Vec3{-0.25}} + ); + + consumer.consumeTorque( + state, + getConnectee("force_target"), + SimTK::Vec3{0.1, 0.25, 0.5} + ); + + consumer.consumeGeneralizedForce( + state, + getConnectee("generalized_force_target"), + 2.0 + ); + + consumer.consumePointForce( + state, + getConnectee("force_target"), + SimTK::Vec3{1.0, 2.0, 3.0}, + SimTK::Vec3{-1.0, -3.0, -9.0} + ); + } + }; + + // A trivial implementation of a `Force` that can be added to a `Model` + // for the purposes of comparing side-effects to the `ForceProducer` API + // (see `MockForceProducer` above). + class MockForce final : public Force { + OpenSim_DECLARE_CONCRETE_OBJECT(MockForce, Force); + public: + OpenSim_DECLARE_SOCKET(force_target, OpenSim::PhysicalFrame, "the physical frame that forces should be produced for"); + OpenSim_DECLARE_SOCKET(generalized_force_target, OpenSim::Coordinate, "the coordinate that generalized forces should be produced for"); + + explicit MockForce( + const OpenSim::PhysicalFrame& forceTarget, + const OpenSim::Coordinate& generalizedForceTarget) + { + connectSocket_force_target(forceTarget); + connectSocket_generalized_force_target(generalizedForceTarget); + } + + void computeForce( + const SimTK::State& state, + SimTK::Vector_& bodyForces, + SimTK::Vector& generalizedForces) const override + { + // Note: this should be logically equivalent to `MockForceProducer::implProduceForces` + // (above), so that the `ForceProducer`'s default `computeForce` implementations can + // be compared in an end-to-end way. + + // (this is usually how legacy `Force` code adds `SimTK::SpatialVec`s to the body forces) + bodyForces[getConnectee("force_target").getMobilizedBodyIndex()] += + SimTK::SpatialVec{SimTK::Vec3{1.0}, SimTK::Vec3{2.0}}; + bodyForces[getConnectee("force_target").getMobilizedBodyIndex()] += + SimTK::SpatialVec{SimTK::Vec3{-0.5}, SimTK::Vec3{-0.25}}; + + applyTorque( + state, + getConnectee("force_target"), + SimTK::Vec3{0.1, 0.25, 0.5}, + bodyForces + ); + + applyGeneralizedForce( + state, + getConnectee("generalized_force_target"), + 2.0, + generalizedForces + ); + + applyForceToPoint( + state, + getConnectee("force_target"), + SimTK::Vec3{1.0, 2.0, 3.0}, + SimTK::Vec3{-1.0, -3.0, -9.0}, + bodyForces + ); + } + }; + + // Returns true if the contents of `a` and `b` are equal, that is, they have the same + // number of elements, and each element in `a` compares equal with the element in `b` + // at the same position. + template + bool equals(const SimTK::Vector_& a, const SimTK::Vector_& b) + { + if (a.size() != b.size()) { + return false; + } + for (int i = 0; i < a.size(); ++i) { + if (a[i] != b[i]) { + return false; + } + } + return true; + } + + // Represents the vectors of forces the simbody physics engine lets downstream + // code (e.g. `OpenSim::Force`s) manipulate + // + // They're bundled together in this struct for ease of use, initialization, and comparison. + struct SimbodyEngineForceVectors { + + // Constructs a zero-initialized set of vectors. + explicit SimbodyEngineForceVectors(const SimTK::SimbodyMatterSubsystem& matter) : + bodyForces{matter.getNumBodies(), SimTK::SpatialVec{SimTK::Vec3{0.0}, SimTK::Vec3{0.0}}}, + particleForces{matter.getNumParticles(), SimTK::Vec3{0.0}}, + mobilityForces{matter.getNumMobilities(), double{}} + {} + + friend bool operator==(const SimbodyEngineForceVectors& lhs, const SimbodyEngineForceVectors& rhs) + { + return + equals(lhs.bodyForces, rhs.bodyForces) && + equals(lhs.particleForces, rhs.particleForces) && + equals(lhs.mobilityForces, rhs.mobilityForces); + } + + friend bool operator!=(const SimbodyEngineForceVectors& lhs, const SimbodyEngineForceVectors& rhs) + { + return !(lhs == rhs); + } + + SimTK::Vector_ bodyForces; + SimTK::Vector_ particleForces; // unused by OpenSim + SimTK::Vector mobilityForces; + }; +} + +TEST_CASE("ForceProducer (ExampleForceProducer)") +{ + SECTION("Can Default Construct `ExampleForceProducer`") + { + // This test is mostly just ensuring that the example components for + // the test suite behave correctly. That is, inheriting from the + // `ForceProducer` API should work in trivial cases + + const ExampleForceProducer example; + } + + SECTION("Passing `ExampleForceConsumer` to default-constructed `ExampleForceProducer` produces no forces") + { + // This test is checking that the API lets calling code pass an example + // consumer into an example producer. It's ensuring that the concrete + // public API talks to the relevant virtual APIs in this trivial (0-force) + // case. + + const ExampleForceProducer producer; + ExampleForceConsumer consumer; + const SimTK::State state; // untouched by example classes + producer.produceForces(state, consumer); + + REQUIRE(consumer.getNumGeneralizedForcesConsumed() == 0); + REQUIRE(consumer.getNumBodySpatialVectorsConsumed() == 0); + REQUIRE(consumer.getNumPointForcesConsumed() == 0); + } + + SECTION("Passing Non-Zero Number of Generalized Forces to `ExampleForceProducer` makes it produce stubbed generalized forces into the `ExampleForceConsumer`") + { + // This test is checking that a generalized force produced by an (example) + // `ForceProducer` will correctly worm its way into an (example) `ForceConsumer` + + const size_t numGeneralizedForcesToProduce = 7; + const ExampleForceProducer producer{numGeneralizedForcesToProduce, 0, 0}; + + ExampleForceConsumer consumer; + const SimTK::State state; // untouched by example classes + producer.produceForces(state, consumer); + + REQUIRE(consumer.getNumGeneralizedForcesConsumed() == numGeneralizedForcesToProduce); + REQUIRE(consumer.getNumBodySpatialVectorsConsumed() == 0); + REQUIRE(consumer.getNumPointForcesConsumed() == 0); + } + + SECTION("Passing Non-Zero Number of Body Spatial Vectors to `ExampleForceProducer` makes it produce stubbed spatial vectors into the `ExampleForceConsumer`") + { + // This test is checking that a body spatial vector produced by an (example) + // `ForceProducer` will correctly worm its way into an example `ForceConsumer` + const size_t numBodySpatialVectorsToProduce = 9; + const ExampleForceProducer producer{0, numBodySpatialVectorsToProduce, 0}; + + ExampleForceConsumer consumer; + const SimTK::State state; // untouched by example classes + producer.produceForces(state, consumer); + + REQUIRE(consumer.getNumGeneralizedForcesConsumed() == 0); + REQUIRE(consumer.getNumBodySpatialVectorsConsumed() == numBodySpatialVectorsToProduce); + REQUIRE(consumer.getNumPointForcesConsumed() == 0); + } + + SECTION("Passing Non-Zero Number of Point Force Vectors to `ExampleForceProducer` makes it produce stubbed point forces into the `ExampleForceConsumer`") + { + // This test is checking that a body spatial vector produced by an (example) + // `ForceProducer` will correctly worm its way into an example `ForceConsumer` + const size_t numPointForcesToProduce = 11; + const ExampleForceProducer producer{0, 0, numPointForcesToProduce}; + + ExampleForceConsumer consumer; + const SimTK::State state; // untouched by example classes + producer.produceForces(state, consumer); + + REQUIRE(consumer.getNumGeneralizedForcesConsumed() == 0); + REQUIRE(consumer.getNumBodySpatialVectorsConsumed() == 0); + REQUIRE(consumer.getNumPointForcesConsumed() == numPointForcesToProduce); + } + + SECTION("Setting `produceForces` to `false` Causes `ExampleForceProducer` to Still Produce Forces") + { + // This test is checking that `appliesForce`, which `ForceProducer` inherits from the + // `Force` base class, is ignored by the `ForceProducer` API. The wording, + // "APPLIES force" implies that the flag should only be checked during force + // application, not production. + + ExampleForceProducer producer{1, 2, 3}; // produce nonzero number of forces + producer.set_appliesForce(false); // from `OpenSim::Force` + producer.finalizeFromProperties(); + + ExampleForceConsumer consumer; + const SimTK::State state; // untouched by example classes + producer.produceForces(state, consumer); + + REQUIRE(consumer.getNumGeneralizedForcesConsumed() == 1); + REQUIRE(consumer.getNumBodySpatialVectorsConsumed() == 2); + REQUIRE(consumer.getNumPointForcesConsumed() == 3); + } + + SECTION("The `ForceProducer` class's default `computeForce` Implementation Works as Expected") + { + // The `ForceProducer` base class provides a default implementation of `Force::computeForce`, + // which should behave identically to it in the case where the downstream code uses the + // `ForceConsumer` API "identially" (logically speaking) to the `Force` API. + // + // For example, when a concrete implementation calls `ForceConsumer::consumePointForce` in + // its `implProduceForces` implementation during a call to `ForceProducer::computeForce`, + // that should have identical side-effects as when a concrete implementation calls + // `Force::applyForceToPoint` during a call to `Force::computeForce` (assuming the + // same arguments, conditions, etc.). + // + // I.e. "The `ForceProducer::computeForce` API should behave logically identically to the + // `Force::computeForce` API. It's just that the `ForceProducer` API allows for + // switching consumers' behavior. + + // step 1) build a model with "equivalent" `ForceProducer` and `Force` implementations + Model model; + auto* body = new Body{"body", 1.0, SimTK::Vec3{0.0}, SimTK::Inertia{1.0}}; + auto* joint = new FreeJoint{"joint", model.getGround(), *body}; + auto* force = new MockForce{*body, joint->get_coordinates(0)}; + auto* forceProducer = new MockForceProducer{*body, joint->get_coordinates(0)}; + model.addBody(body); + model.addJoint(joint); + model.addForce(force); + model.addForce(forceProducer); + model.buildSystem(); + model.initializeState(); + + // step 2) create zero-initialized force vectors "as if" pretending to be the physics engine + SimbodyEngineForceVectors blankForceVectors{model.getMatterSubsystem()}; + SimbodyEngineForceVectors forceVectors{model.getMatterSubsystem()}; + SimbodyEngineForceVectors forceProducerVectors{model.getMatterSubsystem()}; + + // step 3a) pump one set of the vectors through the `Force` implementation + { + ForceAdapter adapter{*force}; + adapter.calcForce(model.getWorkingState(), forceVectors.bodyForces, forceVectors.particleForces, forceVectors.mobilityForces); + } + // step 3b) pump the other set of vectors through the `ForceProducer` implementation + { + ForceAdapter adapter{*forceProducer}; + adapter.calcForce(model.getWorkingState(), forceProducerVectors.bodyForces, forceProducerVectors.particleForces, forceProducerVectors.mobilityForces); + } + + // step 4) compare the vector sets, which should be equal if `ForceProducer` behaves the same + // as `Force` for typical use-cases + REQUIRE((forceVectors != blankForceVectors && forceVectors == forceProducerVectors)); + } + + SECTION("Setting `appliesForces` to `false` Causes `ForceProducer::computeForce` to Not Apply Forces") + { + // The base `OpenSim::Force` class has an `appliesForces` flag, which `ForceProducer` + // should check when it wants to APPLY forces to the multibody system (i.e. during + // `ForceProducer::computeForce` override to satisfy the `OpenSim::Force` API) + Model model; + auto* body = new Body{"body", 1.0, SimTK::Vec3{0.0}, SimTK::Inertia{1.0}}; + auto* joint = new FreeJoint{"joint", model.getGround(), *body}; + auto* forceProducer = new MockForceProducer{*body, joint->get_coordinates(0)}; + forceProducer->set_appliesForce(false); // should disable force application + model.addBody(body); + model.addJoint(joint); + model.addForce(forceProducer); + model.buildSystem(); + model.initializeState(); + + SimbodyEngineForceVectors blankForceVectors{model.getMatterSubsystem()}; + SimbodyEngineForceVectors forceProducerVectors{model.getMatterSubsystem()}; + + ForceAdapter adapter{*forceProducer}; + adapter.calcForce(model.getWorkingState(), forceProducerVectors.bodyForces, forceProducerVectors.particleForces, forceProducerVectors.mobilityForces); + + REQUIRE(forceProducerVectors == blankForceVectors); + } +} diff --git a/OpenSim/Simulation/Test/testStatesDocument.cpp b/OpenSim/Simulation/Test/testStatesDocument.cpp new file mode 100644 index 0000000000..698efccde3 --- /dev/null +++ b/OpenSim/Simulation/Test/testStatesDocument.cpp @@ -0,0 +1,775 @@ +/* -------------------------------------------------------------------------- * +* OpenSim: testComponentInterface.cpp * +* -------------------------------------------------------------------------- * +* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * +* See http://opensim.stanford.edu and the NOTICE file for more information. * +* OpenSim is developed at Stanford University and supported by the US * +* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * +* through the Warrior Web program. * +* * +* Copyright (c) 2024 Stanford University and the Authors * +* Author(s): F. C. Anderson * +* * +* Licensed under the Apache License, Version 2.0 (the "License"); you may * +* not use this file except in compliance with the License. You may obtain a * +* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * +* * +* Unless required by applicable law or agreed to in writing, software * +* distributed under the License is distributed on an "AS IS" BASIS, * +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * +* See the License for the specific language governing permissions and * +* limitations under the License. * +* -------------------------------------------------------------------------- */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace SimTK; +using namespace OpenSim; +using std::cout; +using std::endl; +using std::string; +using std::vector; + + +// Internal static methods and classes. +namespace +{ + +// Constant used to determine equality tolerances +const double padFactor = 1.0 + SimTK::SignificantReal; + + +//----------------------------------------------------------------------------- +// Create a force component derived from PointToPointSpring that adds a +// discrete state of each supported type (bool, int, double, Vec2, Vec3, +// Vec4, Vec5, Vec6). +class ExtendedPointToPointSpring : public OpenSim::PointToPointSpring +{ + OpenSim_DECLARE_CONCRETE_OBJECT(ExtendedPointToPointSpring, + OpenSim::PointToPointSpring); + +private: + // Subsystem index + SubsystemIndex indexSS; + // Indexes of discrete variables + DiscreteVariableIndex indexBool; + DiscreteVariableIndex indexInt; + DiscreteVariableIndex indexDbl; + DiscreteVariableIndex indexVec2; + DiscreteVariableIndex indexVec3; + DiscreteVariableIndex indexVec4; + DiscreteVariableIndex indexVec5; + DiscreteVariableIndex indexVec6; + // Names of discrete variables + string nameBool{"dvBool"}; + string nameInt{"dvInt"}; + string nameDbl{"dvDbl"}; + string nameVec2{"dvVec2"}; + string nameVec3{"dvVec3"}; + string nameVec4{"dvVec4"}; + string nameVec5{"dvVec5"}; + string nameVec6{"dvVec6"}; + // Omit a discrete state altogether + int omit; + +public: + + // Constructor + // @param which Specify which discrete state name (0 to 7) to append the + // suffix to. + // @param suffix String to append to the discrete state name. + // @param omit Specify the discrete state to omit. + ExtendedPointToPointSpring(const PhysicalFrame& body1, SimTK::Vec3 point1, + const PhysicalFrame& body2, SimTK::Vec3 point2, + double stiffness, double restlength, + int which = -1, const string& suffix = "", int omit = -1) : + PointToPointSpring(body1, point1, body2, point2, stiffness, restlength), + omit(omit) + { + switch (which) { + case(0) : + nameBool += suffix; + break; + case(1) : + nameInt += suffix; + break; + case(2) : + nameDbl += suffix; + break; + case(3) : + nameVec2 += suffix; + break; + case(4) : + nameVec3 += suffix; + break; + case(5) : + nameVec4 += suffix; + break; + case(6) : + nameVec5 += suffix; + break; + case(7) : + nameVec6 += suffix; + break; + } + } + + void + extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const override + { + Super::extendAddToSystemAfterSubcomponents(system); + + // Add the discrete state to the list of OpenSim Components + // For exception testing purposes, the member variable 'omit' is used + // to omit one state. + bool allocate = false; + if(omit!=0) addDiscreteVariable(nameBool, Stage::Position, allocate); + if(omit!=1) addDiscreteVariable(nameInt, Stage::Position, allocate); + if(omit!=2) addDiscreteVariable(nameDbl, Stage::Position, allocate); + if(omit!=3) addDiscreteVariable(nameVec2, Stage::Position, allocate); + if(omit!=4) addDiscreteVariable(nameVec3, Stage::Position, allocate); + if(omit!=5) addDiscreteVariable(nameVec4, Stage::Position, allocate); + if(omit!=6) addDiscreteVariable(nameVec5, Stage::Position, allocate); + if(omit!=7) addDiscreteVariable(nameVec6, Stage::Position, allocate); + } + + void + extendRealizeTopology(SimTK::State& s) const override + { + Super::extendRealizeTopology(s); + + // Create a mutableThis + ExtendedPointToPointSpring* mutableThis = + const_cast(this); + + // Get the Subsystem + const DefaultSystemSubsystem& fsub = getModel().getDefaultSubsystem(); + mutableThis->indexSS = fsub.getMySubsystemIndex(); + + // 0 Bool + if(omit != 0) { + bool dvBool{false}; + mutableThis->indexBool = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvBool), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameBool, indexSS, indexBool); + } + + // 1 Int + if(omit != 1) { + int dvInt{0}; + mutableThis->indexInt = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvInt), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameInt, indexSS, indexInt); + } + + // 2 Dbl + if(omit != 2) { + double dvDbl{0.0}; + mutableThis->indexDbl = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvDbl), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameDbl, indexSS, indexDbl); + } + + // 3 Vec2 + if(omit != 3) { + Vec2 dvVec2(0.1, 0.2); + mutableThis->indexVec2 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec2), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec2, indexSS, indexVec2); + } + + // 4 Vec3 + if(omit != 4) { + Vec3 dvVec3(0.1, 0.2, 0.3); + mutableThis->indexVec3 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec3), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec3, indexSS, indexVec3); + } + + // 5 Vec4 + if(omit != 5) { + Vec4 dvVec4(0.1, 0.2, 0.3, 0.4); + mutableThis->indexVec4 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec4), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec4, indexSS, indexVec4); + } + + // 6 Vec5 + if(omit != 6) { + Vec5 dvVec5(0.1, 0.2, 0.3, 0.4, 0.5); + mutableThis->indexVec5 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec5), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec5, indexSS, indexVec5); + } + + // 7 Vec6 + if(omit != 7) { + Vec6 dvVec6(0.1, 0.2, 0.3, 0.4, 0.5, 0.6); + mutableThis->indexVec6 = + s.allocateAutoUpdateDiscreteVariable(indexSS, + Stage::Velocity, new Value(dvVec6), Stage::Dynamics); + initializeDiscreteVariableIndexes(nameVec6, indexSS, indexVec6); + } + } + + // Set the values of the discrete variables. + // The actual force calculation is done in SimTK::TwoPointLinearSpring. + // This method just provides a means of setting the added discrete + // variables so that they change during the course of a simulation. + // The discrete variables are just set to the generalized speeds. + virtual void computeForce(const SimTK::State& state, + SimTK::Vector_& bodyForces, + SimTK::Vector& generalizedForces) const override + { + Super::computeForce(state, bodyForces, generalizedForces); + + const SimTK::Vector& u = state.getU(); + + // 0 Bool + if (omit != 0) { + bool& vBool = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexBool)); + vBool = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexBool); + } + + // 1 Int + if (omit != 1) { + SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexInt)) = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexInt); + } + + // 2 Dbl + if (omit != 2) { + SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexDbl)) = u[0]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexDbl); + } + + // 3 Vec2 + if (omit != 3) { + Vec2& v2 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec2)); + v2[0] = u[0]; + v2[1] = u[1]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec2); + } + + // 4 Vec3 + if (omit != 4) { + Vec3& v3 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec3)); + v3[0] = u[0]; + v3[1] = u[1]; + v3[2] = u[2]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec3); + } + + // 5 Vec4 + if (omit != 5) { + Vec4& v4 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec4)); + v4[0] = u[0]; + v4[1] = u[1]; + v4[2] = u[2]; + v4[3] = u[3]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec4); + } + + // 6 Vec5 + if (omit != 6) { + Vec5& v5 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec5)); + v5[0] = u[0]; + v5[1] = u[1]; + v5[2] = u[2]; + v5[3] = u[3]; + v5[4] = u[4]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec5); + } + + // 7 Vec6 + if (omit != 7) { + Vec6& v6 = SimTK::Value::downcast( + state.updDiscreteVarUpdateValue(indexSS, indexVec6)); + v6[0] = u[0]; + v6[1] = u[1]; + v6[2] = u[2]; + v6[3] = u[3]; + v6[4] = u[4]; + v6[5] = u[5]; + state.markDiscreteVarUpdateValueRealized(indexSS, indexVec6); + } + } + +}; // End of class ExtendedPointToPointSpring + + +//----------------------------------------------------------------------------- +// Other Local Static Methods +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +/** +Compute the maximum error that can result from rounding a value at a +specified precision. This method assumes a base-10 representation of the value. +@param value Value to be rounded. +@param precision Number of significant figures that will be retained in the +value. +@return Maximum rounding error. + +double +computeMaxRoundingError(double value, int precision) { + if (value == 0) return 0.0; + int p = clamp(1, precision, SimTK::LosslessNumDigitsReal); + double leastSigDigit = trunc(log10(fabs(value))-precision); + double max_eps = 0.5*pow(10.0, leastSigDigit); + if(max_eps < SimTK::LeastPositiveReal) return SimTK::LeastPositiveReal; + return max_eps; +} +*/ // No longer used, but might be useful elsewhere, so saving. + +//_____________________________________________________________________________ +/** +Compute the expected error that will occur as a result of rounding a value at +a specified precision. +@param value Value to be rounded. +@param precision Number of significant figures that will be retained in the +value. +@return Expected rounding error. +*/ +double +computeRoundingError(const double& value, int precision) { + int p = clamp(1, precision, SimTK::LosslessNumDigitsReal); + SimTK::String valueStr(value, precision); + double valueDbl; + if(!valueStr.tryConvertToDouble(valueDbl)) + cout << "Conversion to double failed" << endl; + return fabs(valueDbl - value); +} + +//_____________________________________________________________________________ +// Test for equality of the continuous variables in two state trajectories. +void +testEqualityForContinuousVariables(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + // Continuous variables are gathered efficiently without using any + // OpenSim::Component methods by using state.getQ(), state.getU(), and + // state.getZ(). + double tol; + double tA, tB; + const State* stateA = trajA.cbegin(); + const State* stateB = trajB.cbegin(); + + // Loop over time + for(int iTime=0; stateA!=trajA.cend(); ++iTime, ++stateA, ++stateB) { + + // Check subsystem consistency + // This checks that basic parameters like number of subystem, nq, nu, + // and nz are the same for two state objects. + REQUIRE(stateA->isConsistent(*stateB)); + + // Get time + tA = stateA->getTime(); + tB = stateB->getTime(); + tol = padFactor * computeRoundingError(tA, precision); + CHECK_THAT(tB, Catch::Matchers::WithinAbs(tA, tol)); + + // Check the number of subsystesm + int nsubA = stateA->getNumSubsystems(); + int nsubB = stateB->getNumSubsystems(); + REQUIRE(nsubA == nsubB); + + // Q + double diff; + const Vector& qA = stateA->getQ(); + const Vector& qB = stateB->getQ(); + int nq = qA.size(); + for (int i = 0; i < nq; ++i) { + tol = padFactor * computeRoundingError(qA[i], precision); + CHECK_THAT(qB[i], Catch::Matchers::WithinAbs(qA[i], tol)); + } + // U + const Vector& uA = stateA->getU(); + const Vector& uB = stateB->getU(); + int nu = uA.size(); + for (int i = 0; i < nu; ++i) { + tol = padFactor * computeRoundingError(uA[i], precision); + CHECK_THAT(uB[i], Catch::Matchers::WithinAbs(uA[i], tol)); + } + // Z + const Vector& zA = stateA->getZ(); + const Vector& zB = stateB->getZ(); + int nz = zA.size(); + for (int i = 0; i < nz; ++i) { + tol = padFactor * computeRoundingError(zA[i], precision); + CHECK_THAT(zB[i], Catch::Matchers::WithinAbs(zA[i], tol)); + } + } +} + +//_____________________________________________________________________________ +// Test for equality of a scalar variable in two state trajectories. +template +void +checkScalar(const Array_& a, const Array_& b, int precision) +{ + double tol; + Array_ dvA, dvB; + for (size_t i = 0; i < (size_t)dvA.size(); ++i) { + tol = padFactor*computeRoundingError(a[i], precision); + CHECK_THAT(b[i], Catch::Matchers::WithinAbs(a[i], tol)); + } +} + +//_____________________________________________________________________________ +// Test for equality of a Vector variable in two state trajectories. +template +void +checkVector(const Array_& a, const Array_& b, int precision) +{ + double tol; + for (size_t i = 0; i < (size_t)a.size(); ++i) { + for (size_t j = 0; j < (size_t)a[i].size(); ++j) { + tol = padFactor*computeRoundingError(a[i][j], precision); + CHECK_THAT(b[i][j], Catch::Matchers::WithinAbs(a[i][j], tol)); + } + } +} + +//_____________________________________________________________________________ +// Test the equality of the discrete variables. +// +// The SimTK API does not allow an exhaustive, low-level comparison of +// discrete variables on the SimTK side. +// +// The comparision is done only for the discrete variables registered +// in the OpenSim Component heirarchy. Any discrete variable that is +// not registered in OpenSim will not be serialized, deserialized, or +// compared in this unit test. +void +testEqualityForDiscreteVariables(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + // Loop over the named variables + OpenSim::Array paths = model.getDiscreteVariableNames(); + int nPaths = paths.getSize(); + for (int i = 0; i < nPaths; ++i) { + + // Get one variable so that its type can be ascertained. + const AbstractValue& abstractVal = + model.getDiscreteVariableAbstractValue(trajA[i],paths[i]); + + // Get the trajectory for the discrete variable + if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + for (size_t j = 0; j < (size_t)dvA.size(); ++j) + CHECK(dvB[j] == dvA[j]); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + for (size_t j = 0; j < (size_t)dvA.size(); ++j) + CHECK(dvB[j] == dvA[j]); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkScalar(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkScalar(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else if (SimTK::Value::isA(abstractVal)) { + Array_ dvA, dvB; + model.getDiscreteVariableTrajectory(paths[i], trajA, dvA); + model.getDiscreteVariableTrajectory(paths[i], trajB, dvB); + checkVector(dvA, dvB, precision); + } + else { + String msg = "Unrecognized type: " + abstractVal.getTypeName(); + SimTK_ASSERT(false, msg.c_str()); + } + } +} + +//_____________________________________________________________________________ +// Test the equality of the modeling options. +// +// The SimTK API does not allow an exhaustive, low-level comparison of +// modeling options on the SimTK side. +// +// The comparision is done only for the modeling options registered +// in the OpenSim Component heirarchy. Any modeling option that is +// not registered in the OpenSim Component hierarchy will not be serialized, +// deserialized, or compared. +void +testEqualityForModelingOptions(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + // Loop over the named variables + OpenSim::Array paths = model.getModelingOptionNames(); + int nPaths = paths.getSize(); + for (int i = 0; i < nPaths; ++i) { + Array_ moA, moB; + model.getModelingOptionTrajectory(paths[i], trajA, moA); + model.getModelingOptionTrajectory(paths[i], trajB, moB); + for (size_t j = 0; j<(size_t)moA.size(); ++j) CHECK(moB[j] == moA[j]); + } +} + +//_____________________________________________________________________________ +// Test for equality of two state trajectories. +// If a state variable fails an equality test, it is likely that that +// variable has not been added to OpenSim's Component heirarchy and therefore +// has not been serialized. +void +testStateEquality(const Model& model, + const Array_& trajA, const Array_& trajB, int precision) +{ + testEqualityForContinuousVariables(model, trajA, trajB, precision); + testEqualityForDiscreteVariables(model, trajA, trajB, precision); + testEqualityForModelingOptions(model, trajA, trajB, precision); +} + +//_____________________________________________________________________________ +// Build the model +Model* +buildModel(int whichDiscreteState = -1, + const string& discreteStateSuffix = "", int omit = -1) { + + // Create an empty model + Model* model = new Model(); + Vec3 gravity(0.0, -10.0, 0.0); + model->setGravity(gravity); + model->setName("BlockOnASpringFreeJoint"); + + // Add bodies and joints + OpenSim::Ground& ground = model->updGround(); + + // Body + std::string name = "block"; + OpenSim::Body* block = new OpenSim::Body(); + double mass = 10.0; + block->setName(name); + block->set_mass(mass); + block->set_mass_center(Vec3(0)); + block->setInertia(Inertia(1.0)); + model->addBody(block); + + // Joint + name = "free"; + FreeJoint *free = new + FreeJoint(name, ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + model->addJoint(free); + + // Point-To-Point Spring + // This actuator connects the origin of the block to the orgin of the + // coordinate frame. + double kp = 1000.0; // Stiffness + double kv = 100.0; // Viscosity (under-damped) + double restlength = 0.0; + Vec3 origin(0.0); + Vec3 insertion(0.1, 0.1, 0.025); + ExtendedPointToPointSpring* spring = new ExtendedPointToPointSpring( + ground, origin, *block, insertion, kp, restlength, + whichDiscreteState, discreteStateSuffix, omit); + model->addForce(spring); + + return model; +} + +//_____________________________________________________________________________ +// Simulate +SimTK::Array_ +simulate(Model* model) { + + // Add a StatesTrajectoryReporter + // The reporter records the SimTK::State in a SimTK::Array_<> at a + // specified time interval. + OpenSim::StatesTrajectoryReporter* reporter = + new StatesTrajectoryReporter(); + reporter->setName("states_reporter"); + double interval = 0.1; + reporter->set_report_time_interval(interval); + model->addComponent(reporter); + + // Build the system + model->buildSystem(); + SimTK::State& state = model->initializeState(); + + // Integrate + Manager manager(*model); + manager.getIntegrator().setMaximumStepSize(0.01); + manager.setIntegratorAccuracy(1.0e-5); + double ti = 0.0; + double tf = 5.0; + state.setTime(ti); + manager.initialize(state); + state = manager.integrate(tf); + + // Return a copy of the underlying state array, after repackaging it + // as a SimTK::Array_. + const vector& trajectory = reporter->getVectorOfStateObjects(); + const Array_ traj(trajectory); + return traj; +} + +} // End anonymous namespace + + +TEST_CASE("Serialization and Deserialization") +{ + // Build the model and run a simulation. + // The output of simulate() is the state trajectory. + // Note that a copy of the state trajectory is returned, so we don't have + // to worry about the reporter (or any other object) going out of scope + // or being deleted. + Model *model = buildModel(); + Array_ traj = simulate(model); + + // Serialize + SimTK::String filename = "BlockOnASpring.ostates"; + SimTK::String note = "Output from `testStatesDocument.cpp`."; + for (int p = 1; p < 22; ++p) { + cout << "Testing for precision = " << p << endl; + + StatesDocument doc(*model, traj, note, p); + doc.serialize(filename); + + // (A) Deserialize + StatesDocument docA(filename); + Array_ trajA; + docA.deserialize(*model, trajA); + + // Check the note and the precision. + CHECK(docA.getNote() == doc.getNote()); + CHECK(docA.getPrecision() == doc.getPrecision()); + + // Check size + REQUIRE(traj.size() == traj.size()); + + // Realize both state trajectories to Stage::Report + for (size_t i = 0; i < (size_t)trajA.size(); ++i) { + model->getSystem().realize(traj[i], Stage::Report); + model->getSystem().realize(trajA[i], Stage::Report); + } + + // Are state trajectories A and B the same? + testStateEquality(*model, traj, trajA, p); + } + + delete model; +} + +TEST_CASE("Exceptions") +{ + // Build the default model and run a simulation + Model *model = buildModel(); + Array_ traj = simulate(model); + + // Serialize the default model + SimTK::String filename = "BlockOnASpring.ostates"; + SimTK::String note = "Output from `testStatesDocument.cpp`."; + int precision = 6; + StatesDocument doc(*model, traj, note, precision); + doc.serialize(filename); + + // (A) Model names don't match + const string& name = model->getName(); + model->setName(name + "_diff"); + StatesDocument docA(filename); + Array_ trajA; + CHECK_THROWS(docA.deserialize(*model, trajA), + "Model names should not match."); + model->setName(name); // return the original name + + // (B) A discrete state is not found because no name matches + // In each model, the name of one discrete state is changed. + string suffix{"_ShouldNotBeFound"}; + for (int which = 0; which < 8; ++which) { + cout << "Changing the name of discrete state " << which << endl; + + // Build a model that is different only with respect to one name of a + // specified discrete state. + Model* modelB = buildModel(which, suffix); + Array_ trajDoNotNeed = simulate(modelB); + + // Deserialize using modelB + // This should fail when name of a discrete state has been changed. + StatesDocument docB(filename); + Array_ trajB; + CHECK_THROWS(docB.deserialize(*modelB, trajB), + "Discrete state should not be found"); + + delete modelB; + } + + // (C) A discrete state is not found because the state doesn't exist. + // An exception should be thrown because the number of states don't match. + for (int which = -1, omit = 0; omit < 8; ++omit) { + cout << "Omitting discrete state " << omit << endl; + + // Build a model that is different only in that one discrete state + // is omitted. + Model* modelC = buildModel(which, suffix, omit); + Array_ trajDoNotNeed = simulate(modelC); + + // Deserialize using modelC + StatesDocument docC(filename); + Array_ trajC; + CHECK_THROWS(docC.deserialize(*modelC, trajC), + "Expected number of discrete states should be wrong"); + + delete modelC; + } + + delete model; +} \ No newline at end of file diff --git a/OpenSim/Simulation/Test/testStatesTrajectory.cpp b/OpenSim/Simulation/Test/testStatesTrajectory.cpp index 649eb310bf..aa150dc55a 100644 --- a/OpenSim/Simulation/Test/testStatesTrajectory.cpp +++ b/OpenSim/Simulation/Test/testStatesTrajectory.cpp @@ -67,13 +67,13 @@ void testPopulateTrajectoryAndStatesTrajectoryReporter() { const double finalTime = 0.05; { auto& state = model.initSystem(); - + SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem()); SimTK::TimeStepper ts(model.getSystem(), integrator); ts.initialize(state); ts.setReportAllSignificantStates(true); integrator.setReturnEveryInternalStep(true); - + StatesTrajectory states; std::vector times; while (ts.getState().getTime() < finalTime) { @@ -84,7 +84,7 @@ void testPopulateTrajectoryAndStatesTrajectoryReporter() { // For the StatesTrajectoryReporter: model.getMultibodySystem().realize(ts.getState(), SimTK::Stage::Report); } - + // Make sure we have all the states SimTK_TEST_EQ((int)states.getSize(), (int)times.size()); SimTK_TEST_EQ((int)statesCol->getStates().getSize(), (int)times.size()); @@ -149,7 +149,7 @@ void createStateStorageFile() { // histories. auto* controller = new PrescribedController(); // For consistent results, use same seed each time. - std::default_random_engine generator(0); + std::default_random_engine generator(0); // Uniform distribution between 0.1 and 0.9. std::uniform_real_distribution distribution(0.1, 0.8); @@ -399,7 +399,7 @@ void testFromStatesStorageUniqueColumnLabels() { Model model("gait2354_simbody.osim"); Storage sto(statesStoFname); - + // Edit column labels so that they are not unique. auto labels = sto.getColumnLabels(); labels[10] = labels[7]; @@ -478,7 +478,7 @@ void testFromStatesStoragePre40CorrectStates() { // Fiber length. SimTK_TEST_EQ( - getStorageEntry(sto, itime, muscleName + ".fiber_length"), + getStorageEntry(sto, itime, muscleName + ".fiber_length"), muscle.getFiberLength(state)); // More complicated computation based on state. @@ -570,16 +570,20 @@ void testBoundsCheck() { states.append(state); states.append(state); states.append(state); - + #ifdef NDEBUG // In DEBUG, Visual Studio puts asserts into the index operator. states[states.getSize() + 100]; states[4]; states[5]; #endif - SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange); - SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100), - IndexOutOfRange); + + // SimTK::Array_<> only throws exceptions when in a DEBUG build + #ifdef DEBUG + SimTK_TEST_MUST_THROW_EXC(states.get(4), IndexOutOfRange); + SimTK_TEST_MUST_THROW_EXC(states.get(states.getSize() + 100), + IndexOutOfRange); + #endif } void testIntegrityChecks() { @@ -676,7 +680,7 @@ void testIntegrityChecks() { } // TODO Show weakness of the test: two models with the same number of Q's, U's, - // and Z's both pass the check. + // and Z's both pass the check. } void tableAndTrajectoryMatch(const Model& model, diff --git a/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp b/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp index 4dcaf4a697..9a5d98e34d 100644 --- a/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp +++ b/OpenSim/Simulation/Wrap/WrapEllipsoid.cpp @@ -696,7 +696,7 @@ int WrapEllipsoid::calcTangentPoint(double p1e, SimTK::Vec3& r1, SimTK::Vec3& p1 } for (i = 0; i < 4; i++) - diag[i] = dedth2[i][i]; + diag[i] = dedth2(i, i); nit2 = 0; diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 5672961ccd..8c7e87534b 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -28,6 +28,11 @@ #include "Model/Bhargava2004SmoothedMuscleMetabolics.h" #include "Model/Model.h" #include "Model/ModelVisualizer.h" +#include "Model/Force.h" +#include "Model/ForceAdapter.h" +#include "Model/ForceApplier.h" +#include "Model/ForceConsumer.h" +#include "Model/ForceProducer.h" #include "Model/ForceSet.h" #include "Model/BodyScale.h" #include "Model/BodyScaleSet.h" diff --git a/OpenSim/Tools/Test/CMakeLists.txt b/OpenSim/Tools/Test/CMakeLists.txt index a9ad9d72ec..2dac8afae6 100644 --- a/OpenSim/Tools/Test/CMakeLists.txt +++ b/OpenSim/Tools/Test/CMakeLists.txt @@ -9,3 +9,8 @@ OpenSimAddTests( DATAFILES ${TEST_FILES} LINKLIBS osimTools Catch2::Catch2WithMain ) + + +# edge-case: one test ensures that `ExternalLoads` still works - even when it isn't in +# the working directory of the test runner (#3926) +file(COPY "ExternalLoadsInSubdir" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}") diff --git a/OpenSim/Tools/Test/ExternalLoadsInSubdir/external-loads-in-subdir.xml b/OpenSim/Tools/Test/ExternalLoadsInSubdir/external-loads-in-subdir.xml new file mode 100644 index 0000000000..1b125b1492 --- /dev/null +++ b/OpenSim/Tools/Test/ExternalLoadsInSubdir/external-loads-in-subdir.xml @@ -0,0 +1,27 @@ + + + + + + + false + + new_body + + new_body + + new_body + + head_v + + head_p + + + Unassigned + + + + + forces-in-subdir.mot + + diff --git a/OpenSim/Tools/Test/ExternalLoadsInSubdir/forces-in-subdir.mot b/OpenSim/Tools/Test/ExternalLoadsInSubdir/forces-in-subdir.mot new file mode 100644 index 0000000000..80f90f9dca --- /dev/null +++ b/OpenSim/Tools/Test/ExternalLoadsInSubdir/forces-in-subdir.mot @@ -0,0 +1,9 @@ +pendulum_swing +version=1 +nRows=2 +nColumns=7 +inDegrees=yes +endheader +time head_vx head_vy head_vz head_px head_py head_pz +0 0 0 0 0 0 0 +1 1000 1000 1000 0 0 0 diff --git a/OpenSim/Tools/Test/ExternalLoadsInSubdir/model-in-subdir.osim b/OpenSim/Tools/Test/ExternalLoadsInSubdir/model-in-subdir.osim new file mode 100644 index 0000000000..3091f5cdb9 --- /dev/null +++ b/OpenSim/Tools/Test/ExternalLoadsInSubdir/model-in-subdir.osim @@ -0,0 +1,115 @@ + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.10000000000000001 + + + + 1 + + 0 0 0 + + 1 1 1 0 0 0 + + + + + + + + + + ground_offset + + new_body_offset + + + + + 1.5707963705062866 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /ground + + 0 1 0 + + -0 0 -0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/new_body + + -0 0.46722877025604248 -0 + + -0 0 -0 + + + + + + + + + + + + + + + + + + + + + + true + + + + diff --git a/OpenSim/Tools/Test/testExternalLoads.cpp b/OpenSim/Tools/Test/testExternalLoads.cpp index d5478453cc..105ec25517 100644 --- a/OpenSim/Tools/Test/testExternalLoads.cpp +++ b/OpenSim/Tools/Test/testExternalLoads.cpp @@ -21,24 +21,15 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -#include #include #include -using namespace OpenSim; -using namespace std; - -void testExternalLoad(); -void testExternalLoadDefaultProperties(); +#include -int main() -{ - SimTK_START_TEST("testExternalLoads"); - SimTK_SUBTEST(testExternalLoad); - SimTK_SUBTEST(testExternalLoadDefaultProperties); - SimTK_END_TEST(); -} +#include +using namespace OpenSim; +using namespace std; void addLoadToStorage(Storage &forceStore, SimTK::Vec3 force, SimTK::Vec3 point, SimTK::Vec3 torque) { @@ -85,7 +76,7 @@ void addLoadToStorage(Storage &forceStore, SimTK::Vec3 force, SimTK::Vec3 point, forces->addToRdStorage(forceStore, 0.0, 1.0); } -void testExternalLoad() +TEST_CASE("ExternalLoads") { using namespace SimTK; @@ -249,7 +240,8 @@ void testExternalLoad() } // Ensure the default values for the ExternalForce properties work as expected. -void testExternalLoadDefaultProperties() { +TEST_CASE("ExternalLoads Default Properties") +{ using namespace SimTK; Model model("Pendulum.osim"); @@ -303,3 +295,18 @@ void testExternalLoadDefaultProperties() { xf->set_force_expressed_in_body("nonexistent"); model.initSystem(); } + +// related: #3926 +// +// adding a valid `OpenSim::ExternalLoads` to a model shouldn't result in a model +// that cannot be copied. +TEST_CASE("ExternalLoads Can Be Copied") +{ + OpenSim::Model model{"ExternalLoadsInSubdir/model-in-subdir.osim"}; + model.finalizeConnections(); // should work + model.addModelComponent(&dynamic_cast(*Object::makeObjectFromFile("ExternalLoadsInSubdir/external-loads-in-subdir.xml"))); + model.finalizeConnections(); // should also work + + OpenSim::Model copy{model}; // create an independent copy containing the `OpenSim::ExternalLoads` + copy.finalizeConnections(); // should work (wasn't when this test was written) +} diff --git a/Vendors/tropter/CMakeLists.txt b/Vendors/tropter/CMakeLists.txt index bea44576a6..b25d662dd7 100644 --- a/Vendors/tropter/CMakeLists.txt +++ b/Vendors/tropter/CMakeLists.txt @@ -13,8 +13,8 @@ set_property(GLOBAL PROPERTY USE_FOLDERS ON) # Compiler flags. # --------------- -set(CMAKE_CXX_STANDARD 11) -# Using c++11 is not optional. +set(CMAKE_CXX_STANDARD 14) +# Using c++14 is not optional. set(CMAKE_CXX_STANDARD_REQUIRED ON) # Copy dependencies' libraries into tropter's installation? diff --git a/Vendors/tropter/README.md b/Vendors/tropter/README.md index a344a6fa5b..0c003dfc9d 100644 --- a/Vendors/tropter/README.md +++ b/Vendors/tropter/README.md @@ -10,7 +10,7 @@ Goals 1. Detailed and helpful error messages and diagnostics (visualizing a sparsity pattern, inspecting constraint violations). -2. A simple and modern (C++11) interface for specifying the optimal control +2. A simple and modern (c++14) interface for specifying the optimal control problem. 3. Users do not need to supply derivative information (gradient, Jacobian, diff --git a/Vendors/tropter/cmake/CMakeLists.txt b/Vendors/tropter/cmake/CMakeLists.txt index 355093dcc3..a62eecaea9 100644 --- a/Vendors/tropter/cmake/CMakeLists.txt +++ b/Vendors/tropter/cmake/CMakeLists.txt @@ -1,27 +1,33 @@ if(TROPTER_COPY_DEPENDENCIES AND APPLE) - get_filename_component(gcc_libdir "${pkgcfg_lib_IPOPT_gfortran}" DIRECTORY) - file(GLOB gfortran "${gcc_libdir}/libgfortran*.dylib") - file(GLOB quadmath "${gcc_libdir}/libquadmath*.dylib") + # Extract actual name of the library libgfortran, gcc, quadmath to install along on osx + # these show only as dependencies of libipopt, and libgcc is in same folder as libgfortran + # not very robust but works for our artifacts. + execute_process(COMMAND bash "-c" + "otool -L ${IPOPT_LIBDIR}/libipopt.dylib | grep 'libgfortran' | awk '{print $1}'" + OUTPUT_VARIABLE libgfortran_name + OUTPUT_STRIP_TRAILING_WHITESPACE) - # Get the actual name of the library libgcc_s.1.dylib (can be libgcc_s.1.1.dylib), - # or another if it changes in the future. + string(REPLACE "libgfortran.5" "libgcc_s.1.1" libgcc_name ${libgfortran_name}) + execute_process(COMMAND bash "-c" - "basename `otool -L ${IPOPT_LIBDIR}/libipopt.dylib | grep 'libgcc' | awk '{print $1}'`" - OUTPUT_VARIABLE libgcc_name + "otool -L ${IPOPT_LIBDIR}/libipopt.dylib | grep 'libquadmath' | awk '{print $1}'" + OUTPUT_VARIABLE libquadmath_name OUTPUT_STRIP_TRAILING_WHITESPACE) install(FILES ${ADOLC_DIR}/lib64/libadolc.2.dylib ${ADOLC_DIR}/lib64/libadolc.dylib - # /usr/local/opt/boost/lib/libboost_system.dylib ${ColPack_ROOT_DIR}/lib/libColPack.0.dylib ${IPOPT_LIBDIR}/libipopt.3.dylib ${IPOPT_LIBDIR}/libipopt.dylib - ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.3.dylib - ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libmetis.dylib + ${IPOPT_LIBDIR}/libcoinmumps.3.dylib + ${IPOPT_LIBDIR}/libcoinmumps.dylib + ${IPOPT_LIBDIR}/libmetis.dylib + ${libgfortran_name} + ${libquadmath_name} + ${libgcc_name} DESTINATION ${CMAKE_INSTALL_LIBDIR}) @@ -59,10 +65,10 @@ elseif(TROPTER_COPY_DEPENDENCIES AND UNIX) ${IPOPT_LIBDIR}/libipopt.so.3.14.16 ${IPOPT_LIBDIR}/libipopt.so.3 ${IPOPT_LIBDIR}/libipopt.so - ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so.3.0.5 - ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so.3 - ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so - ${IPOPT_LIBDIR}/../../metis/lib/libmetis.so + ${IPOPT_LIBDIR}/libcoinmumps.so.3.0.5 + ${IPOPT_LIBDIR}/libcoinmumps.so.3 + ${IPOPT_LIBDIR}/libcoinmumps.so + ${IPOPT_LIBDIR}/libmetis.so ${gfortran} ${quadmath} diff --git a/Vendors/tropter/cmake/tropter_install_mac_dependency_libraries.cmake.in b/Vendors/tropter/cmake/tropter_install_mac_dependency_libraries.cmake.in index cf9e211c1f..8f73576564 100644 --- a/Vendors/tropter/cmake/tropter_install_mac_dependency_libraries.cmake.in +++ b/Vendors/tropter/cmake/tropter_install_mac_dependency_libraries.cmake.in @@ -76,9 +76,6 @@ execute_process(COMMAND bash "-c" # tropter install_name_tool_change(tropter adolc.2 "@ADOLC_DIR@/lib64") install_name_tool_change(tropter ipopt.3 "@IPOPT_LIBDIR@") -install_name_tool_change(tropter coinmumps.3 "@IPOPT_LIBDIR@") -install_name_tool_change(tropter coinmetis.1 "@IPOPT_LIBDIR@") -install_name_tool_add_rpath(tropter) # adol-c install_name_tool_id(adolc.2) @@ -88,15 +85,20 @@ install_name_tool_delete_rpath(adolc.2 "@ColPack_ROOT_DIR@/lib") # ipopt install_name_tool_id(ipopt.3) install_name_tool_change(ipopt.3 coinmumps.3 "@IPOPT_LIBDIR@") -install_name_tool_change(ipopt.3 coinmetis.1 "@IPOPT_LIBDIR@") +install_name_tool_change(ipopt.3 metis "@IPOPT_LIBDIR@") +install_name_tool_change_gfortran(ipopt.3) +install_name_tool_change_quadmath(ipopt.3) # coinmumps install_name_tool_id(coinmumps.3) -install_name_tool_change(coinmumps.3 coinmetis.1 "@IPOPT_LIBDIR@") +install_name_tool_change(coinmumps.3 metis "@IPOPT_LIBDIR@") install_name_tool_change(coinmumps.3 "${libgcc_name}" "${libgcc_dir}") install_name_tool_add_rpath(coinmumps.3) +install_name_tool_change_gfortran(coinmumps.3) +install_name_tool_change_quadmath(coinmumps.3) -# coinmetis -install_name_tool_id(coinmetis.1) -install_name_tool_add_rpath(coinmetis.1) +# metis +install_name_tool_id(metis) +install_name_tool_add_rpath(metis) + diff --git a/Vendors/tropter/tests/test_derivatives.cpp b/Vendors/tropter/tests/test_derivatives.cpp index b20979ab38..2dfa55c4cd 100644 --- a/Vendors/tropter/tests/test_derivatives.cpp +++ b/Vendors/tropter/tests/test_derivatives.cpp @@ -656,8 +656,9 @@ TEST_CASE("User-supplied sparsity of Hessian of Lagrangian") const auto& i = hes_sparsity.row[inz]; const auto& j = hes_sparsity.col[inz]; INFO(inz << " (" << i << " " << j << ")"); - REQUIRE(analytical_hessian(i, j) == - Approx(actual_hessian_values[inz]).epsilon(1e-5)); + REQUIRE_THAT(analytical_hessian(i, j), + Catch::Matchers::WithinAbs( + actual_hessian_values[inz], 1e-5)); } } { diff --git a/cmake/OpenSimMacros.cmake b/cmake/OpenSimMacros.cmake index 6ce3edc9c5..e1b9e537e9 100644 --- a/cmake/OpenSimMacros.cmake +++ b/cmake/OpenSimMacros.cmake @@ -221,6 +221,11 @@ function(OpenSimAddLibrary) # This target links to the libraries provided as arguments to this func. target_link_libraries(${OSIMADDLIB_LIBRARY_NAME} ${OSIMADDLIB_LINKLIBS}) + target_compile_definitions(${OSIMADDLIB_LIBRARY_NAME} PUBLIC + # `spdlog` transitively uses a deprecated `stdext::checked_array_iterator` + $<$:_SILENCE_STDEXT_ARR_ITERS_DEPRECATION_WARNING> + ) + # This is for exporting classes on Windows. if(OSIMADDLIB_VENDORLIB) set(OSIMADDLIB_FOLDER "Vendor Libraries") diff --git a/cmake/SampleCMakeLists.txt b/cmake/SampleCMakeLists.txt index 9ee0fc4c57..a18ce01394 100644 --- a/cmake/SampleCMakeLists.txt +++ b/cmake/SampleCMakeLists.txt @@ -11,7 +11,7 @@ project(myexe) set(my_source_files myexe.cpp) set(my_header_files myexe.h) -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # This depends on OpenSimConfig.cmake being located somewhere predictable diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 7ecea1ffa0..c89eec0314 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -105,7 +105,6 @@ function(AddDependency) list(APPEND CMAKE_ARGS -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}) endif() - list(APPEND CMAKE_ARGS ${DEP_CMAKE_ARGS}) # Forward cmake arguments to dependencies. list(APPEND CMAKE_ARGS @@ -140,6 +139,9 @@ function(AddDependency) list(APPEND CMAKE_ARGS -DSWIG_EXECUTABLE:FILEPATH=${SWIG_EXECUTABLE}) endif() + # Append the dependency-specific CMake arguments. + list(APPEND CMAKE_ARGS ${DEP_CMAKE_ARGS}) + if(DEP_GIT_URL) ExternalProject_Add(${DEP_NAME} DEPENDS ${DEP_DEPENDS} @@ -183,7 +185,7 @@ AddDependency(NAME ezc3d AddDependency(NAME simbody DEFAULT ON GIT_URL https://github.com/simbody/simbody.git - GIT_TAG d98f05fa1899633b544757e6c179562940f0849d + GIT_TAG f853397efad00798a169ff3dd1c52f5e20a24ba9 CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF -DBUILD_TESTING:BOOL=OFF ${SIMBODY_EXTRA_CMAKE_ARGS}) @@ -193,6 +195,11 @@ AddDependency(NAME docopt GIT_URL https://github.com/docopt/docopt.cpp.git GIT_TAG 3dd23e3280f213bacefdf5fcb04857bf52e90917) +set(SPDLOG_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +if(MSVC) + # `spdlog` transitively uses a deprecated `stdext::checked_array_iterator` + set(SPDLOG_CXX_FLAGS "${SPDLOG_CXX_FLAGS} /D_SILENCE_STDEXT_ARR_ITERS_DEPRECATION_WARNING") +endif() AddDependency(NAME spdlog DEFAULT ON @@ -201,7 +208,8 @@ AddDependency(NAME spdlog CMAKE_ARGS -DSPDLOG_BUILD_BENCH:BOOL=OFF -DSPDLOG_BUILD_TESTS:BOOL=OFF -DSPDLOG_BUILD_EXAMPLE:BOOL=OFF - -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON) + -DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON + -DCMAKE_CXX_FLAGS:STRING=${SPDLOG_CXX_FLAGS}) AddDependency(NAME catch2 DEFAULT ON @@ -325,9 +333,9 @@ else() ExternalProject_Add(metis # URL http://glaros.dtc.umn.edu/gkhome/fetch/sw/metis/metis-5.1.0.tar.gz URL https://sourceforge.net/projects/myosin/files/metis-5.1.0.tar.gz/download - CONFIGURE_COMMAND cd && ${CMAKE_MAKE_PROGRAM} shared=1 config "prefix=${CMAKE_INSTALL_PREFIX}/metis" BUILDDIR=bdir + CONFIGURE_COMMAND cd && ${CMAKE_MAKE_PROGRAM} shared=1 config "prefix=${CMAKE_INSTALL_PREFIX}/ipopt" BUILDDIR=bdir BUILD_COMMAND cd /bdir && ${CMAKE_MAKE_PROGRAM} - INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/metis" + INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/ipopt" INSTALL_COMMAND cd /bdir && ${CMAKE_MAKE_PROGRAM} install) # GCC 10 generates a warning when compling MUMPS that we must suppress using @@ -338,7 +346,7 @@ else() PATCH_COMMAND cd && ./get.Mumps CONFIGURE_COMMAND /configure --prefix= ADD_FFLAGS=-fallow-argument-mismatch ADD_CFLAGS=-Wno-error=implicit-function-declaration BUILD_COMMAND ${CMAKE_MAKE_PROGRAM} ${BUILD_FLAGS} - INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/mumps" + INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/ipopt" INSTALL_COMMAND ${CMAKE_MAKE_PROGRAM} install) ExternalProject_Add(ipopt @@ -346,7 +354,7 @@ else() URL https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.16.zip INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/ipopt" # Suppress warnings treated as errors in Clang/LLVM with -Wno-error=implicit-function-declaration - CONFIGURE_COMMAND /configure --with-mumps --prefix= --with-mumps-cflags="-I${CMAKE_INSTALL_PREFIX}/mumps/include/coin-or/mumps" --with-mumps-lflags="-L${CMAKE_INSTALL_PREFIX}/mumps/lib -lcoinmumps" + CONFIGURE_COMMAND /configure --with-mumps --prefix= --with-mumps-cflags="-I${CMAKE_INSTALL_PREFIX}/ipopt/include/coin-or/mumps" --with-mumps-lflags="-L${CMAKE_INSTALL_PREFIX}/ipopt/lib -lcoinmumps" BUILD_COMMAND ${CMAKE_MAKE_PROGRAM} ${BUILD_FLAGS} INSTALL_COMMAND ${CMAKE_MAKE_PROGRAM} install) endif() diff --git a/doc/Moco/MocoStudy.dox b/doc/Moco/MocoStudy.dox index 4fd8ab2c81..4524406fad 100644 --- a/doc/Moco/MocoStudy.dox +++ b/doc/Moco/MocoStudy.dox @@ -159,6 +159,32 @@ A MocoSolver attempts to use an optimal control method to solve the MocoProblem, and exposes settings you can use to control the transcription to a generic optimization problem and the optimization process. +@subsection transcriptionschemes Transcription schemes + +Moco supports multiple transcription schemes which can be specified +via the `transcription_scheme` property when configuring a MocoSolver. +For example, here's how to specify the trapezoidal transcription scheme: + +@code{.cpp} +MocoCasADiSolver& solver = moco.initCasADiSolver(); +solver.set_transcription_scheme("trapezoidal"); +@endcode + +The transcription schemes supported by Moco are summarized in the table below: + +| Scheme | `transcription_scheme` setting | Theory | +|----------------------|--------------------------------|-----------------------------------| +| trapezoidal | "trapezoidal" | @ref mocotraptheory | +| Hermite-Simpson | "hermite-simpson" | @ref mocohermitesimpsontheory | +| Legendre-Gauss | "legendre-gauss-#" | @ref mocolegendregausstheory | +| Legendre-Gauss-Radau | "legendre-gauss-radau-#" | @ref mocolegendregaussradautheory | + +For the Legendre-Gauss and Legendre-Gauss-Radau transcription schemes, the `#` +in the setting string should be replaced by a number between [1, 9], which +represents the number of collocation points per mesh interval (this is also +equal to the degree of the interpolating Lagrange polynomials used in these +schemes). + @subsection implicitmultibody Implicit multibody dynamics mode MocoSolvers support two modes for expressing differential equations for @@ -189,40 +215,79 @@ solver.set_enforce_constraint_derivatives(true); solver.set_lagrange_multiplier_weight(10); @endcode -If constraint derivatives are enforced, velocity correction variables are added -to the problem according to the method described in Posa, Kuindersma, and -Tedrake, 2016, "Optimization and stabilization of trajectories for constrained -dynamical systems." - -@note Enforcing kinematic constraints is only supported with Hermite-Simpson -transcription, since this scheme is necessary for implementing the method -described in Posa et al. 2016. See @ref mocohermitesimpsontheory for a -mathematical description. - -It is possible to modify the default bounds on these velocity correction -variables: +Moco supports two methods for enforcing kinematic constraints based on the +following publications: +- Posa, Kuindersma, and Tedrake, 2016, "Optimization and stabilization of + trajectories for constrained dynamical systems," and the +- Bordalba, Schoels, Ros, Porta, and Diehl, 2023, "Direct Collocation Methods + for Trajectory Optimization in Constraint Robotic Systems." + +Hermite-Simpson is the only transcription scheme supported when enforcing +kinematic constraints with the method by Posa et al. 2016. See @ref +mocohermitesimpsontheory for a mathematical description. The kinematic +constraint method by Bordalba et al. 2023 is only supported when using +MocoCasADiSolver and requires that constraint derivatives are enforced. +See @ref mocolegendregausstheory for a mathematical description. Note that +the Bordalba et al. 2023 method is valid for all transcription schemes, +not just Legendre-Gauss collocation. The implementation for each method +has been slighly modified from the original publications to be compatible +with Simbody; we refer to the mathematical descriptions linked above which +describe the implementations used by Moco. + +When constraint derivatives are enforced, slack variables are added to the +problem for projecting the model coordinates back onto the constraint manifold. +These variables also prevent the problem from being overconstrained. The Posa et +al. 2016 method adds a set of "velocity correction" variables that are applied +to coordinate state derivatives at the midpoint collocation point in the +Hermite-Simpson method. The Bordalba et al. 2023 method uses slack variables in +a set of constraints that enforce the coordinate states to be an orthogonal +projection of the states trajectory obtained from enforcing the defect constraints. + +The default method is the method by Posa et al. 2016 since this was originally +the only kinematic constraint method supported in Moco. However, we recommend +using the method by Bordalba et al. 2023 since it is agnostic to the transcription +scheme and does not require modifying the state derivative values as in the +Posa et al. 2016 method. + +It is possible to modify the default bounds on the slack variables used by both +methods. For the velocity correction variables in the Posa et al. 2016 method: @code{.cpp} // Default bounds: [-0.1, 0.1] solver.set_velocity_correction_bounds({-0.25, 0.25}); @endcode +For the slack variables in the Bordalba et al. 2023 method: + +@code{.cpp} +const MocoCasADiSolver& solver = moco.initCasADiSolver(); +// Default bounds: [-1e-3, 1e-3] +solver.set_projection_slack_variable_bounds({-0.05, 0.05}); +@endcode + +The slack variables in both methods are multiplied by model's kinematic +constraint Jacobian to provide a small projection back onto the constraint +manifold. Therefore, the slack values should be very small in a converged +solution, so in most cases the default bounds should be sufficient. + If constraint derivatives are not enforced or if the multibody constraint Jacobian is rank-deficient, it is recommended that the Lagrange multiplier minimization term is added to the problem to impose uniqueness for these variables. -Even though OpenSim Models can contain non-holonomic constraints (that is, -\f$ \nu(q, u, p) = 0 \f$), Moco solvers support only holonomic constraints; that -is, \f$ \phi(q) = 0 \f$. +When using the method by Posa et al. 2016, we only support enforcing holonomic +constraints; that is \f$ \phi(q, p) = 0 \f$. The method by Bordalba et al. 2023 +also supports both non-holonomic constraints (that is, \f$ \nu(q, u, p) = 0 \f$) +and acceleration-only constraints (that is, \f$ \alpha(q, u, \dot u, p) = 0 \f$), +but these constraints are less commonly used in OpenSim models. -By default, the controls at mesh interval midpoints are constrained by linear -interpolation of control mesh endpoint values and is the recommended setting. +By default, the controls within the mesh interval interior are constrained by +linear interpolation of control mesh endpoint values and is the recommended setting. However, you may disable this behavior in the solver: \code{.cpp} MocoCasADiSolver& solver = moco.initCasADiSolver(); -solver.set_interpolate_control_midpoints(false); +solver.set_interpolate_control_mesh_interior_points(false); \endcode @section casadivstropter MocoCasADiSolver versus MocoTropterSolver diff --git a/doc/Moco/MocoTheoryGuide.dox b/doc/Moco/MocoTheoryGuide.dox index c171f70fbd..8d7ca34c28 100644 --- a/doc/Moco/MocoTheoryGuide.dox +++ b/doc/Moco/MocoTheoryGuide.dox @@ -152,6 +152,15 @@ see the following excellent material: https://doi.org/10.1137/16M1062569 - Textbook: Betts, J. T. (2010). Practical methods for optimal control and estimation using nonlinear programming (Vol. 19). Siam. + - Journal article: Bordalba, R., Schoels, T., Ros, L., Porta J. M., and + Diehl, M. (2023). Direct collocation methods for trajectory optimization + in constrained robotic systems. IEEE Transations on Robotics, 39(1), 183-202. + https://doi.org/10.1109/TRO.2022.3193776 + - Thesis: Benson, A. (2005). A Gauss pseudospectral transcription for optimal + control. PhD thesis, Massachusetts Institute of Technology. + - Thesis: Huntington, G.T. (2007). Advancement and analysis of a Gauss + pseudospectral transcription for optimal control. PhD thesis, Massachusetts + Institute of Technology. @subsection mocotraptheory Trapezoidal transcription @@ -170,7 +179,7 @@ We use the trapezoidal rule to remove integrals and derivatives from the original continuous time problem, \f[ - \textrm{trap}_i(F(\eta, p)) = \frac{1}{2} h_i (F(\eta_{i-1}, p) + F(\eta_i, p)) + \textrm{trap}_i(F(t, \eta, p)) = \frac{1}{2} h_i (F(t_{i-1}, \eta_{i-1}, p) + F(t_i, \eta_i, p)) \f] where \f$ \eta \f$ is any continuous variable in the optimal control problem @@ -197,29 +206,32 @@ finite-dimensional nonlinear optimization problem (or nonlinear program, abbrevi & \quad\quad S_{b,k} = \sum_{i=1}^{n} \textrm{trap}_i(s_{b,k}(t, y, x, \lambda, p)) && k = 1, \ldots, K \\ & g_{L} \leq g(t_i, y_i, x_{i}, \lambda_i, p) \leq g_{U} && i = 0, \ldots, n\\ \mbox{with respect to} \quad - & t_0 \in [t_{0,L}, t_{0,U}] && t_n \in [t_{f,L}, t_{f,U}] \\ - & y_0 \in [y_{0,L}, y_{0,U}] && y_n \in [y_{f,L}, y_{f,U}] \\ - & y_i \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1\\ + & t_0 \in [t_{0,L}, t_{0,U}] \\ + & t_n \in [t_{f,L}, t_{f,U}] \\ + & y_0 \in [y_{0,L}, y_{0,U}] \\ + & y_n \in [y_{f,L}, y_{f,U}] \\ + & y_i \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1 \\ & \zeta_i \in [\zeta_{L}, \zeta_{U}] && i = 0, \ldots, n \\ - & x_0 \in [x_{0,L}, x_{0,U}] && x_n \in [x_{f,L}, x_{f,U}] \\ - & x_i \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1\\ + & x_0 \in [x_{0,L}, x_{0,U}] \\ + & x_n \in [x_{f,L}, x_{f,U}] \\ + & x_i \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1 \\ & \lambda_i \in [\lambda_L, \lambda_U] && i = 0, \ldots, n \\ & p \in [p_{L}, p_{U}]. \end{aligned} \end{align} \f] -Above, the multibody dynamics \f$ f_{\textrm{mb}} \f$ are expressed as explicit +Above, the multibody dynamics \f$ f_{\dot{u}} \f$ are expressed as explicit differential equations: \f[ - f_{\textrm{mb}}(t, y, x, \lambda, p) = + f_{\dot{u}}(t, y, x, \lambda, p) = M(q, p)^{-1}\big[(f_{\textrm{app}}(t, y, x, p) - f_{\textrm{inertial}}(q, u, p) - G(q, p)^T \lambda\big] \f] If the multibody dynamics are enforced as implicit differential equations, then -the constraint involving \f$ f_\textrm{mb} \f$ is replaced with the following: +the constraint involving \f$ f_\dot{u} \f$ is replaced with the following: \f[ \begin{alignat*}{2} @@ -265,8 +277,8 @@ with the Hermite interpolant and Simpson integration rule: \f[ \begin{align} - \textrm{hermite}_i(\eta, F(\eta, p)) &= \frac{1}{2} (\eta_{i-1} + \eta_i) + \frac{h_i}{8} (F(\eta_{i-1}, p) - F(\eta_i, p)) \\ - \textrm{simpson}_i(F(\eta, p)) &= \frac{h_i}{6} (F(\eta_{i-1}, p) + 4 F(\bar{\eta}_i, p) + F(\eta_i, p)) + \textrm{hermite}_i(\eta, F(t, \eta, p)) &= \frac{1}{2} (\eta_{i-1} + \eta_i) + \frac{h_i}{8} (F(t_{i-1}, \eta_{i-1}, p) - F(t_i, \eta_i, p)) \\ + \textrm{simpson}_i(F(t, \eta, p)) &= \frac{h_i}{6} (F(t_{i-1}, \eta_{i-1}, p) + 4 F(\bar{t}_i, \bar{\eta}_i, p) + F(t_i, \eta_i, p)) \end{align} \f] @@ -284,7 +296,7 @@ The resulting finite-dimensional NLP is as follows: + w_{\lambda} \sum_{i=1}^{n} \textrm{simpson}_i(\|\lambda\|_2^2) \\ & \quad\quad S_{c,j} = \sum_{i=1}^{n} \textrm{simpson}_i(s_{c,j}(t, y, x, \lambda, p)) \\ \mbox{subject to} \quad - & \bar{q}_i = \textrm{hermite}_i(q, u) + G(\bar{q}_i, p)^T \bar{\gamma}_i && i = 1, \ldots, n \\ + & \bar{q}_i = \textrm{hermite}_i(q, u) + P(\bar{q}_i, p)^T \bar{\gamma}_i && i = 1, \ldots, n \\ & q_i = q_{i-1} + \textrm{simpson}_i(u) && i = 1, \ldots, n \\ & \bar{u}_i = \textrm{hermite}_i(u, f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ & u_i = u_{i-1} + \textrm{simpson}_i(f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ @@ -296,18 +308,21 @@ The resulting finite-dimensional NLP is as follows: & \bar{x}_i = (x_{i-1} + x_i)/2 && i = 1, \ldots, n \\ & 0 = \phi(q_i, p) && i = 0, \ldots, n\\ & 0 = \dot{\phi}(q_i, u_i, p) && i = 0, \ldots, n\\ - & 0 = \ddot{\phi}(t_i, y_i, x_i, \lambda_i, p) && i = 0, \ldots, n\\ + & 0 = \ddot{\phi}(q_i, u_i, x_i, \lambda_i, p) && i = 0, \ldots, n\\ & V_{L,k} \leq V_k(t_0, t_f, y_0, y_f, x_{0}, x_{f}, \lambda_0, \lambda_f, p, S_{b,k}) \leq V_{U,k} \\ & \quad\quad S_{b,k} = \sum_{i=1}^{n} \textrm{simpson}_i(s_{b,k}(t, y, x, \lambda, p)) && k = 1, \ldots, K \\ & g_{L} \leq g(t_i, y_i, x_{i}, \lambda_i, p) \leq g_{U} && i = 0, \ldots, n\\ \mbox{with respect to} \quad - & t_0 \in [t_{0,L}, t_{0,U}] && t_n \in [t_{f,L}, t_{f,U}] \\ - & y_0 \in [y_{0,L}, y_{0,U}] && y_n \in [y_{f,L}, y_{f,U}] \\ + & t_0 \in [t_{0,L}, t_{0,U}] \\ + & t_n \in [t_{f,L}, t_{f,U}] \\ + & y_0 \in [y_{0,L}, y_{0,U}] \\ + & y_n \in [y_{f,L}, y_{f,U}] \\ & y_i \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1 \\ & \bar{y}_i \in [y_{L}, y_{U}] && i = 1, \ldots, n \\ & \zeta_i \in [\zeta_{L}, \zeta_{U}] && i = 0, \ldots, n \\ & \bar{\zeta}_i \in [\zeta_{L}, \zeta_{U}] && i = 1, \ldots, n \\ - & x_0 \in [x_{0,L}, x_{0,U}] && x_n \in [x_{f,L}, x_{f,U}] \\ + & x_0 \in [x_{0,L}, x_{0,U}] \\ + & x_n \in [x_{f,L}, x_{f,U}] \\ & x_i \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1 \\ & \bar{x}_i \in [x_{L}, x_{U}] && i = 1, \ldots, n \\ & \lambda_i \in [\lambda_L, \lambda_U] && i = 0, \ldots, n \\ @@ -318,34 +333,34 @@ The resulting finite-dimensional NLP is as follows: \end{align} \f] -The explicit multibody dynamics function \f$ f_{\textrm{mb}} \f$ is the same as +The explicit multibody dynamics function \f$ f_{\dot{u}} \f$ is the same as in Trapezoidal transcription. -The \f$ G(\bar{q}_i, p)^T \bar{\gamma}_i \f$ term -represents a velocity correction that is necessary to impose when enforcing -derivatives of kinematic constraints in the NLP. The additional variables -\f$ \bar{\gamma}_i \f$ help avoid problems becoming overconstrained, and the -kinematic Jacobian transpose operator, \f$ G(\bar{q}_i, p)^T \f$, restricts the -velocity correction to the tangent plane of the constraint manifold (see Posa et -al. 2016 described in @ref kincon for more details). Currently, we only support -enforcing derivatives of position-level, or holonomic, constraints, represented -by the equations: +This formulation includes the equations needed to enforce kinematic constraints based +on the method from Posa et al. (2016) (see @ref kincon for more details). The +\f$ P(\bar{q}_i, p)^T \bar{\gamma}_i \f$ term represents a velocity correction that +is necessary to impose when enforcing derivatives of kinematic constraints in the NLP. +The additional variables \f$ \bar{\gamma}_i \f$ help avoid problems becoming +overconstrained, and the kinematic Jacobian transpose operator, \f$ P(\bar{q}_i, p)^T \f$, +restricts the velocity correction to the tangent plane of the constraint manifold. +When using the Posa et al. (2016) method, we only support enforcing derivatives of +position-level, or holonomic, constraints, represented by the equations: \f[ \begin{alignat*}{2} - & 0 = \dot{\phi}(q, u, p) = G(q, p) u\\ - & 0 = \ddot{\phi}(t, y, x, \lambda, p) = G(q, p) f_{\textrm{mb}}(t, y, x, \lambda, p) + \dot{G}(q, p) u \\ + & 0 = \dot{\phi}(q, u, p) = P(q, p) u - c(t, q, p)\\ + & 0 = \ddot{\phi}(q, u, x, \lambda, p) = P(q, p) f_{\dot{u}}(t, y, x, \lambda, p) - b_p(t, q, u, p) \\ \end{alignat*} \f] +Where \f$ P(q, p) \f$ is the Jacobian of the holonomic constraints, which is a submatrix +of the kinematic constraint Jacobian \f$ G(q, p) \f$. When only holonomic constraints are +present in the multibody system, \f$ G = P \f$. + The explicit multibody dynamics function is used here where \f$ \dot{u} \f$ would be if it were a continuous variable in the problem (as is in implicit mode, see below). -Even though OpenSim supports non-holonomic constraints (that is, -\f$ \nu(q, u, p) = 0 \f$), we only support holonomic constraints; that is, -\f$ \phi(q) = 0 \f$. - The implicit dynamics formulation is as follows: \f[ @@ -365,5 +380,282 @@ The implicit dynamics formulation is as follows: \end{alignat*} \f] +@subsection mocolegendregausstheory Legendre-Gauss transcription + +Legendre-Gauss transcription works by approximating the states, \f$ y(t) \f$, within a +mesh interval range \f$ t \in [t_i, t_{i+1}] \f$ using a set of Lagrange interpolating +polynomials of degree \f$ d \f$: + +\f[ + \begin{alignat*}{1} + \mathcal{L}(t, c_i) = y_{i,0} \cdot l_0(t - t_i) + \cdots + y_{i,d} \cdot l_d(t - t_i) + \end{alignat*} +\f] + +where \f$ l_k \f$ are the Lagrange polynomial basis functions and \f$ c_i \f$ is +the vector of interpolated state values which represent the polynomial coefficients: + +\f[ + \begin{alignat*}{1} + c_i = (y_{i,0}, \ldots, y_{i,d}) + \end{alignat*} +\f] + +These polynomials only depend on the time points \f$ t_{i,0}, \ldots, t_{i,d} \f$ and +take on the following values: + +\f[ + \begin{alignat*}{1} + l_j(t_{i,k} - t_i) = \left\{ + \begin{array}{ll} + 1, & \quad \textrm{if } j = k \\ + 0, & \quad \textrm{otherwise} + \end{array} + \right. + \end{alignat*} +\f] + +Therefore, the interpolating polynomial is equal to the discrete state values when +evaluated at the collocation points: + +\f[ + \begin{alignat*}{1} + \mathcal{L}(t_{i,k}, c_i) = y_{i,k} \quad \textrm{for } k = 0, \ldots, d + \end{alignat*} +\f] + +The polynomial at the initial time point of the mesh interval is simply the state variable +at the initial mesh point, \f$ y_{i,0} = y_i \f$. The remaining polynomial coefficients can +be determined by enforcing the \f$ d \f$ collocation constraints at the collocation points +\f$ t_{i,1}, \ldots, t_{i,d} \f$: + +\f[ + \begin{alignat*}{1} + \mathcal{\dot L}_i(t_{i,k}, c_i) = f(t_{i,k}, y_{i,k}, x_{i,k}, \lambda_{i,k}, p) \quad \textrm{for } k = 1, \ldots, d + \end{alignat*} +\f] + +The left-hand side can be formulated using the constant differentiation matrix, \f$ D \f$: + +\f[ + \begin{alignat*}{1} + \mathcal{\dot L}_i(t_{i,k}, c_i) = D(t_{i,0},\ldots,t_{i,d}) \cdot y_i + \end{alignat*} +\f] + +Finally, the end state of the mesh interval, \f$ y_{i+1} \f$, is given by: + +\f[ + \begin{alignat*}{1} + y_{i+1} = \mathcal{L}(t_{i+1}, c_i) + \end{alignat*} +\f] + +As in the previous transcription methods, we have \f$ n + 1 \f$ mesh points and +\f$ n \f$ mesh intervals. Legendre-Gauss transcription introduces \f$ d \f$ +collocation points in the mesh interval interior located at points based on the +roots of the \f$ d \f$-th order Legendre polynomial, \f$ \textrm{legendre_roots}(k) \f$. + +\f[ + \begin{alignat*}{1} + 0 &= \tau_0 < \tau_1 < \tau_2 < \ldots < \tau_i < \ldots < \tau_{n - 1} < \tau_n = 1 \\ + \tau_{i,k} &= \tau_i + (\tau_{i+1} - \tau_i) * \textrm{legendre_roots}(k) \quad \textrm{for } k = 1, \ldots, d \\ + t_i &= (t_f - t_0) \tau_i + t_0 \\ + t_{i,k} &= (t_f - t_0) \tau_{i,k} + t_0 \quad \textrm{for } k = 1, \ldots, d \\ + h_i &= (t_f - t_0)(\tau_i - \tau_{i-1}) \\ + \end{alignat*} +\f] + +The collocation constraint equations and endpoint state equation above can be described using the following defect constraint function: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss}_i(\eta_i, F(t_i, \eta_i, p)) = \begin{bmatrix} h_i * F(t_{i,1:d}, \eta_{i,1:d}, p) - D_i \cdot \eta_{i,0:d} \\ \eta_{i, d+1} - C_i \cdot \eta_{i, 0:d} \end{bmatrix} + \end{alignat*} +\f] + +where \f$ \eta \f$ is any continuous variable in the optimal control problem +(e.g., \f$ y \f$), \f$ F \f$ is any function, \f$ D_i \f$ is the differentiation matrix +described above, and \f$ C_i \f$ a matrix containing values to perform the polynomial +interpolation for \f$ \eta_{i, d+1} \f$. Note that the end state is coincident with time +point \f$ t_{i+1} \f$, but we use the the notation \f$ \eta_{i, d+1} \f$ to denote the end +state of the mesh interval. + +\f$ \eta_{i,0:d} \f$ and \f$ F(t_{i,1:d}, \eta_{i,1:d}, p) \f$ are matrix quantities +with row size based on the polynomial degree \f$ d \f$: + +\f[ + \begin{alignat*}{1} + \eta_{i,0:d} &= \begin{bmatrix} \eta_{i,0} \\ \vdots \\ \eta_{i,d} \end{bmatrix} \quad + F(t_{i,1:d}, \eta_{i,1:d}, p) &= \begin{bmatrix} F(t_{i,1}, \eta_{i,1}, p) \\ \vdots \\ F(t_{i,d}, \eta_{i,d}, p) \end{bmatrix} \quad + \end{alignat*} +\f] + +These are matrix quantities since \f$ \eta_{i,k} \f$ and \f$ F(t_{i,k}, \eta_{i,k}, p) \f$ are +row vectors containing the state variable values and their derivatives, respectively. + +Similarly, integrals are replaced using Gauss quadrature at the Legendre polynomial roots +with coefficients \f$ \omega_k \f$: + +\f[ + \begin{alignat*}{1} + \textrm{quadrature}_i(Q(\eta, p)) = \sum^d_{k=1} \omega_k * Q(\eta_{i,k}, p) + \end{alignat*} +\f] + +For a full description of how the Gauss quadrature coefficients are determined, we refer +the reader to the theses of Benson (2005) and Huntington (2007) mentioned above. + +The resulting finite-dimensional NLP is as follows: + +\f[ +\begin{align} + \begin{aligned} + \mbox{minimize} \quad + & \sum_j w_j J_{j}(t_0, t_f, y_0, y_n, x_{0}, x_{n}, \lambda_0, \lambda_n, p, S_{c,j}) + + w_{\lambda} \sum_{i=1}^{n} \textrm{quadrature}_i(\|\lambda\|_2^2) + + w_{proj} \sum_{i=1}^{n} (\|q_i^{\prime} - q_i\|_2^2 + \|u_i^{\prime} - u_i\|_2^2) \\ + & \quad\quad S_{c,j} = \sum_{i=1}^{n} \textrm{quadrature}_i(s_{c,j}(t, y, x, \lambda, p)) \\ + \mbox{subject to} \quad + & 0 = \textrm{legendre_gauss}^{\prime}_i(q, u) && i = 1, \ldots, n \\ + & 0 = \textrm{legendre_gauss}^{\prime}_i(u, f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ + & 0 = \textrm{legendre_gauss}_i(z_\textrm{ex}, f_{\dot{z},\textrm{ex}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ + & 0 = \textrm{legendre_gauss}_i(z_\textrm{im}, \zeta) && i = 1, \ldots, n \\ + & 0 = f_{\dot{z},\textrm{im}}(t_i, y_i, \zeta_i, x_i, \lambda_i, p) && i = 0, \ldots, n \\ + & 0 = \phi(q_i, p) && i = 0, \ldots, n \\ + & 0 = \dot{\phi}(q_i, u_i, p) && i = 0, \ldots, n \\ + & 0 = \nu(q_i, u_i, p) && i = 0, \ldots, n \\ + & 0 = \ddot{\phi}(q_{i,k}, u_{i,k}, x_{i,k}, \lambda_{i,k}, p) && i = 0, \ldots, n && k = 0, \ldots, d \\ + & 0 = \dot{\nu}(q_{i,k}, u_{i,k}, x_{i,k}, \lambda_{i,k}, p) && i = 0, \ldots, n && k = 0, \ldots, d \\ + & 0 = \alpha(q_{i,k}, u_{i,k}, x_{i,k}, \lambda_{i,k}, p) && i = 0, \ldots, n && k = 0, \ldots, d \\ + & q_{i+1} = q^{\prime}_{i+1} + P_q(q_{i+1},p)^T \mu^{q}_i && i = 0, \ldots, n-1 \\ + & u_{i+1} = u^{\prime}_{i+1} + G_{pv}(q_{i+1},p)^T \mu^{u}_i && i = 0, \ldots, n-1 \\ + & V_{L,k} \leq V_k(t_0, t_f, y_0, y_f, x_{0}, x_{f}, \lambda_0, \lambda_f, p, S_{b,k}) \leq V_{U,k} \\ + & \quad\quad S_{b,k} = \sum_{i=1}^{n} \textrm{quadrature}_i(s_{b,k}(t, y, x, \lambda, p)) && k = 1, \ldots, K \\ + & g_{L} \leq g(t_i, y_i, x_{i}, \lambda_i, p) \leq g_{U} && i = 0, \ldots, n\\ + \mbox{with respect to} \quad + & t_0 \in [t_{0,L}, t_{0,U}] && \\ + & t_n \in [t_{f,L}, t_{f,U}] && \\ + & y_0 \in [y_{0,L}, y_{0,U}] && \\ + & y_n \in [y_{f,L}, y_{f,U}] && \\ + & y_{i,k} \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1 && k = 0, \ldots, d \\ + & \zeta_{i,k} \in [\zeta_{L}, \zeta_{U}] && i = 1, \ldots, n - 1 && k = 0, \ldots, d \\ + & x_0 \in [x_{0,L}, x_{0,U}] && \\ + & x_n \in [x_{f,L}, x_{f,U}] && \\ + & x_{i,k} \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1 && k = 0, \ldots, d \\ + & \lambda_{i,k} \in [\lambda_L, \lambda_U] && i = 0, \ldots, n && k = 0, \ldots, d \\ + & q^{\prime}_{i} \in [q^{\prime}_{L}, q^{\prime}_{U}] && i = 1, \ldots, n \\ + & u^{\prime}_{i} \in [u^{\prime}_{L}, u^{\prime}_{U}] && i = 1, \ldots, n \\ + & \mu^{q}_{i} \in [\mu^{q}_{L}, \mu^{q}_{U}] && i = 1, \ldots, n \\ + & \mu^{u}_{i} \in [\mu^{u}_{L}, \mu^{u}_{U}] && i = 1, \ldots, n \\ + & p \in [p_{L}, p_{U}]. + \end{aligned} +\end{align} +\f] + +This formulation includes the equations needed to enforce kinematic constraints based +on the method from Bordalba et al. (2023) (see @ref kincon for more details). Our +implementation of this method contains one major difference from the original method: +the projection equation (Eq. 35 in the original publication) is separated into the +equations \f$ q_{i+1} = q^{\prime}_{i+1} + P_q(q_{i+1},p)^T \mu^{q}_i \f$ and +\f$ u_{i+1} = u^{\prime}_{i+1} + G_{pv}(q_{i+1},p)^T \mu^{u}_i \f$. This separation is +necessary because Simbody does not (yet) provide submatrices of the matrix \f$ F_x \f$ +from the original method corresponding to the change in velocity-level constraints +with respect to the generalized coordinates (i.e., \f$ \frac{\partial \dot{\phi}}{\partial q} \f$ +and \f$ \frac{\partial \nu}{\partial u} \f$). However, the projection equations +implemented do restrict \f$ q_{i+1} \f$ to be the orthogonal projection of \f$ q^{\prime}_{i+1} \f$ +onto the manifold defined by \f$ \phi = 0 \f$, and restrict \f$ u_{i+1} \f$ to be the +orthogonal projection of \f$ u^{\prime}_{i+1} \f$ onto the manifold defined by +\f$ \dot{\phi} = 0 \f$ and \f$ \nu = 0 \f$. Furthermore, these projections are consistent +with how Simbody enforces constraints via coordinate projection in time-stepping simulations. +If the kinematic constraint equations are enforced and the slack variables \f$ \mu^{q}_i \f$ +and \f$ \mu^{u}_i \f$ are close to zero, then the difference between the true and projection +state variables will be small. + +The Bordalba et al. (2023) method supports holonomic, non-holonomic, and acceleration-only +constraints, but non-holonomic and acceleration-only constraints currently have limited +testing and support in Moco. All acceleration-level constraints (including the derivatives +of holonomic and non-holonomic constraints) are enforced at all collocation points, not +just the mesh points. The cost term preceded by \f$ w_{proj} \f$ is used to penalize the +difference between the true state variables and the projection state variables at the +end of each mesh interval to encourage the closest possible projection to the constraint +manifold. + +The function \f$ \textrm{legendre_gauss}^{\prime}(\eta, F(t, \eta, p)) \f$ used for the +multibody dynamics defect constraints is a slight modification of the function +\f$ \textrm{legendre_gauss} \f$: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss}^{\prime}_i(\eta, F(t, \eta, p)) = \begin{bmatrix} h_i * F(t_i, \eta_i, p) - D_i \cdot \eta_i \\ \eta^{\prime}_{i+1} - C_i \cdot \eta_i \end{bmatrix} + \end{alignat*} +\f] + +where \f$ \eta^{\prime}_{i+1} \f$ represents the "projection" state variables used in the +method from Bordalba et al. (2023). The true state variables \f$ \eta_{i+1} \f$ are the +projection of \f$ \eta^{\prime}_{i+1} \f$ onto the constraint manifold. The matrices +\f$ P_q(q_{i+1},p) \f$ and \f$ G_{pv}(q_{i+1},p) \f$ are used to perform the projection for +the coordinate values, \f$ q \f$, and coordinate speeds, \f$ u \f$, respectively. +\f$ G_{pv} = [P; V] \f$ contains submatrices of the full kinematic constraint Jacobian +(\f$ G = [P; V; A] \f$) for the holonomic, \f$ P \f$, and non-holonomic, \f$ V \f$, +constraints. \f$ Pq = P N^{-1} \f$, where \f$ N \f$ is a matrix relating the generalized +speeds to the generalized coordinate derivatives \f$ \dot q = N u \f$. Currently, Moco +requires that \f$ N = I \f$, but the Bordalba et al. (2023) method will be valid when +Moco supports joints where \f$ \dot q \neq u \f$, in the future. Finally, the vectors +\f$ \mu^{q}_i \f$ and \f$ \mu^{u}_i \f$ are arbitrary slack variables used by the +optimizer to scale the projection magnitude to reach the constraint manifold. + +The implicit dynamics formulation is as follows: + +\f[ + \begin{alignat*}{2} + \mbox{subject to} \quad + & 0 = \textrm{legendre_gauss}^{\prime}_i(u, \upsilon) & i = 1, \ldots, n \\ + & M(q_i, p)\upsilon_i + G(q_i, p)^T \lambda_i = + f_{\textrm{app}}(t_i, y_i, x_{i}, p) - + f_{\textrm{inertial}}(q_i, u_{i}, p) & \quad i = 0, \ldots, n \\ + \mbox{with respect to} \quad + & \upsilon_i \in [-\upsilon_{B}, \upsilon_{B}] & i = 0, \ldots, n \\ + \end{alignat*} +\f] + +@subsection mocolegendregaussradautheory Legendre-Gauss-Radau transcription + +Legendre-Gauss-Radau transcription is very similar to Legendre-Gauss transcription, +with the main difference being that the final collocation point lies at the mesh +interval endpoint rather than the interior of the mesh interval. Both methods have +have \f$ d \f$ collocation points corresponding to the degree of the interpolating +polynomial, but Legendre-Gauss-Radau transcription has one fewer grid point per mesh +interval compared to Legendre-Gauss transcription since the final collocation point +is at the mesh interval endpoint. + +As a result, the defect constraint function no longer needs to explictly include the +interpolation constraint for the final mesh interval state: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss_radau}_i(\eta_i, F(t_i, \eta_i, p)) = h_i * F(t_{i,1:d}, \eta_{i,1:d}, p) - D_i \cdot \eta_{i,0:d} + \end{alignat*} +\f] + +Replacing \f$ \textrm{legendre_gauss}_i \f$ in @ref mocolegendregausstheory above with +\f$ \textrm{legendre_gauss_radau}_i \f$ will yield the equations for Legendre-Gauss-Radau +transcription. The modified equation needed to enforce the kinematic constraints, +\f$ \textrm{legendre_gauss_radau}^{\prime}_i \f$, has the same form, but uses a modified +\f$ \eta^{\prime}_i \f$ matrix where the final row contains the projection state variables: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss_radau}^{\prime}_i(\eta^{\prime}_i, F(t_i, \eta^{\prime}_i, p)) = h_i * F(t_{i,1:d}, \eta^{\prime}_{i,1:d}, p) - D_i \cdot \eta^{\prime}_{i,0:d} + \end{alignat*} +\f] + +\f[ + \begin{alignat*}{1} + \eta^{\prime}_{i,0:d} &= \begin{bmatrix} \eta_{i,0} \\ \vdots \\ \eta^{\prime}_{i,d} \end{bmatrix} \quad + F(t_{i,1:d}, \eta^{\prime}_{i,1:d}, p) &= \begin{bmatrix} F(t_{i,1}, \eta_{i,1}, p) \\ \vdots \\ F(t_{i,d}, \eta^{\prime}_{i,d}, p) \end{bmatrix} \quad + \end{alignat*} +\f] + */ } // namespace OpenSim diff --git a/scripts/build/opensim-core-linux-build-script.sh b/scripts/build/opensim-core-linux-build-script.sh old mode 100644 new mode 100755 index 24f3558fbb..8cbaedb254 --- a/scripts/build/opensim-core-linux-build-script.sh +++ b/scripts/build/opensim-core-linux-build-script.sh @@ -196,7 +196,7 @@ echo echo "LOG: BUILDING OPENSIM-CORE..." mkdir -p ~/opensim-workspace/opensim-core-build || true cd ~/opensim-workspace/opensim-core-build -cmake ~/opensim-workspace/opensim-core-source -G"$GENERATOR" -DOPENSIM_DEPENDENCIES_DIR=~/opensim-workspace/opensim-core-dependencies-install/ -DBUILD_JAVA_WRAPPING=on -DBUILD_PYTHON_WRAPPING=on -DOPENSIM_C3D_PARSER=ezc3d -DBUILD_TESTING=off -DCMAKE_INSTALL_PREFIX=~/opensim-core -DOPENSIM_INSTALL_UNIX_FHS=off -DSWIG_DIR=~/swig/share/swig -DSWIG_EXECUTABLE=~/swig/bin/swig +cmake ~/opensim-workspace/opensim-core-source -G"$GENERATOR" -DOPENSIM_DEPENDENCIES_DIR=~/opensim-workspace/opensim-core-dependencies-install/ -DBUILD_JAVA_WRAPPING=on -DBUILD_PYTHON_WRAPPING=on -DOPENSIM_C3D_PARSER=ezc3d -DBUILD_TESTING=off -DCMAKE_INSTALL_PREFIX=~/opensim-core -DOPENSIM_INSTALL_UNIX_FHS=off -DSWIG_DIR=~/swig/share/swig -DSWIG_EXECUTABLE=~/swig/bin/swig -DOPENSIM_WITH_CASADI=$MOCO -DOPENSIM_WITH_TROPTER=$MOCO cmake . -LAH cmake --build . --config $DEBUG_TYPE -j$NUM_JOBS echo @@ -212,5 +212,5 @@ ctest --parallel $NUM_JOBS --output-on-failure echo "LOG: INSTALL OPENSIM-CORE..." cd ~/opensim-workspace/opensim-core-build cmake --install . -echo 'export PATH=~/opensim-core/bin:$PATH' >> ~/.bashrc -source ~/.bashrc + +cd ~/opensim-core/bin && echo -e "yes" | ./opensim-install-command-line.sh diff --git a/scripts/build/opensim-core-macos-build-script.sh b/scripts/build/opensim-core-macos-build-script.sh index 4d9864d4c5..4d2e47de1a 100644 --- a/scripts/build/opensim-core-macos-build-script.sh +++ b/scripts/build/opensim-core-macos-build-script.sh @@ -112,7 +112,7 @@ echo echo "LOG: BUILDING OPENSIM-CORE..." mkdir -p ~/opensim-workspace/opensim-core-build || true cd ~/opensim-workspace/opensim-core-build -cmake ~/opensim-workspace/opensim-core-source -G"$GENERATOR" -DOPENSIM_DEPENDENCIES_DIR=~/opensim-workspace/opensim-core-dependencies-install/ -DBUILD_JAVA_WRAPPING=on -DBUILD_PYTHON_WRAPPING=on -DOPENSIM_C3D_PARSER=ezc3d -DBUILD_TESTING=off -DCMAKE_INSTALL_PREFIX=~/opensim-core -DOPENSIM_INSTALL_UNIX_FHS=off -DSWIG_DIR=~/swig/share/swig -DSWIG_EXECUTABLE=~/swig/bin/swig -DJAVA_HOME=$(/usr/libexec/java_home -v 1.8) -DJAVA_INCLUDE_PATH=$(/usr/libexec/java_home -v 1.8)/include -DJAVA_INCLUDE_PATH2=$(/usr/libexec/java_home -v 1.8)/include/darwin -DJAVA_AWT_INCLUDE_PATH=$(/usr/libexec/java_home -v 1.8)/include +cmake ~/opensim-workspace/opensim-core-source -G"$GENERATOR" -DOPENSIM_DEPENDENCIES_DIR=~/opensim-workspace/opensim-core-dependencies-install/ -DBUILD_JAVA_WRAPPING=on -DBUILD_PYTHON_WRAPPING=on -DOPENSIM_C3D_PARSER=ezc3d -DBUILD_TESTING=off -DCMAKE_INSTALL_PREFIX=~/opensim-core -DOPENSIM_INSTALL_UNIX_FHS=off -DSWIG_DIR=~/swig/share/swig -DSWIG_EXECUTABLE=~/swig/bin/swig -DJAVA_HOME=$(/usr/libexec/java_home -v 1.8) -DJAVA_INCLUDE_PATH=$(/usr/libexec/java_home -v 1.8)/include -DJAVA_INCLUDE_PATH2=$(/usr/libexec/java_home -v 1.8)/include/darwin -DJAVA_AWT_INCLUDE_PATH=$(/usr/libexec/java_home -v 1.8)/include -DOPENSIM_WITH_CASADI=$MOCO -DOPENSIM_WITH_TROPTER=$MOCO cmake . -LAH cmake --build . --config $DEBUG_TYPE -j$NUM_JOBS echo