diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8e53237e..d889eb6c 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,4 +1,6 @@ name: CI + +# Run on master, tags, or any pull request on: push: branches: @@ -16,20 +18,30 @@ jobs: matrix: version: - '1.6' + - '1.10' - '1' os: - ubuntu-latest - - macOS-latest - - windows-latest arch: - x64 + include: + - os: macOS-latest + version: 1 + arch: arm64 + - os: macOS-latest + version: 1 + arch: x64 + - os: windows-latest + version: 1 + arch: x64 + steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - uses: julia-actions/setup-julia@v1 with: version: ${{ matrix.version }} arch: ${{ matrix.arch }} - - uses: actions/cache@v1 + - uses: actions/cache@v3 env: cache-name: cache-artifacts CI: true @@ -40,15 +52,16 @@ jobs: ${{ runner.os }}-test-${{ env.cache-name }}- ${{ runner.os }}-test- ${{ runner.os }}- - - uses: julia-actions/julia-buildpkg@v1 + - uses: julia-actions/julia-buildpkg@latest - uses: julia-actions/julia-runtest@v1 env: PYTHON: "" JULIA_NUM_THREADS: "2" - uses: julia-actions/julia-processcoverage@v1 - - uses: codecov/codecov-action@v1 + - uses: codecov/codecov-action@v2 with: file: lcov.info + docs: name: Documentation runs-on: ubuntu-latest @@ -56,7 +69,7 @@ jobs: - uses: actions/checkout@v2 - uses: julia-actions/setup-julia@v1 with: - version: '1.6' + version: '1.10' - run: | julia --project=docs -e ' using Pkg diff --git a/.gitignore b/.gitignore index fed71a52..be126296 100644 --- a/.gitignore +++ b/.gitignore @@ -1,10 +1,27 @@ +# Files generated by invoking Julia with --code-coverage *.jl.cov *.jl.*.cov + +# Files generated by invoking Julia with --track-allocation *.jl.mem + +# System-specific files and directories generated by the BinaryProvider and BinDeps packages +# They contain absolute paths specific to the host computer, and so should not be committed deps/deps.jl -*.ipynb_checkpoints +deps/build.log +deps/downloads/ +deps/usr/ +deps/src/ + +# Build artifacts for creating documentation generated by the Documenter package docs/build/ docs/site/ -perf/benchmarkparams.jld -/Manifest.toml +# File generated by Pkg, the package manager, based on a corresponding Project.toml +# It records a fixed state of all packages used by the project. As such, it should not be +# committed for packages, but should be committed for applications that require a static +# environment. +Manifest.toml + +# Jupyter +.ipynb_checkpoints/ diff --git a/Project.toml b/Project.toml index 7df60106..80beb851 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "RigidBodyDynamics" uuid = "366cf18f-59d5-5db9-a4de-86a9f6786172" -version = "2.4.0" +version = "2.5.0" [deps] DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" @@ -19,8 +19,8 @@ UnsafeArrays = "c4a57d5a-5b31-53a6-b365-19f8c011fbd6" DocStringExtensions = "0.4.1, 0.5, 0.6, 0.7, 0.8, 0.9" LightXML = "0.8, 0.9" LoopThrottle = "0.1" -Reexport = "0.2, 1.0" -Rotations = "1.1 - 1.2" +Reexport = "0.2, 1" +Rotations = "1" StaticArrays = "0.8, 0.9, 0.10, 0.11, 0.12, 1" TypeSortedCollections = "1" UnsafeArrays = "1" diff --git a/docs/src/benchmarks.md b/docs/src/benchmarks.md index 6a238113..0b93ecc6 100644 --- a/docs/src/benchmarks.md +++ b/docs/src/benchmarks.md @@ -1,6 +1,14 @@ # Benchmarks -To attain maximal performance, it is recommended to pass `-O3`, `--check-bounds=no` as command line flags to `julia`. As of Julia 1.1, maximizing performance for the `dynamics!` algorithm requires either setting the number of BLAS threads to 1 (`using LinearAlgebra; BLAS.set_num_threads(1)`) if using OpenBLAS (the default), or compiling Julia with MKL. See [this issue](https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/500) for more information. +To attain maximal performance, it is recommended to pass `-O3` and `--check-bounds=no` as command line flags to `julia`: + +```bash +cd RigidBodyDynamics.jl +julia -O3 --check-bounds=no perf/runbenchmarks.jl +``` + +> **Warning** +> For Julia versions previous to `v1.8`, maximizing performance for the `dynamics!` algorithm requires either setting the number of BLAS threads to 1 (`using LinearAlgebra; BLAS.set_num_threads(1)`) if using OpenBLAS (the default), or compiling Julia with MKL. See [this issue](https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/500) for more information. Run `perf/runbenchmarks.jl` to see benchmark results for the Atlas robot (v5). Results below are for the following scenarios: @@ -9,69 +17,62 @@ Run `perf/runbenchmarks.jl` to see benchmark results for the Atlas robot (v5). R 3. Do inverse dynamics. 4. Do forward dynamics. -Note that results on CI builds are **not at all** representative because of code coverage. Results on a reasonably fast laptop at commit [870bea6](https://github.com/JuliaRobotics/RigidBodyDynamics.jl/commit/870bea668d5b11ce0555fa0552592d2c3cb15c54): +> **Note** +> Results on CI builds are **not at all** representative because of code coverage. -Output of `versioninfo()`: +Below are the results for **RBD.jl 2.5.0** (commit [`93a5ea`](https://github.com/JuliaRobotics/RigidBodyDynamics.jl/commit/93a5eaf15a5f6714b1ec1ce621b053542dcb721d)) using **Julia 1.11.1** on an **Apple MacBook Air (M2, 2023)** (16GB RAM, 512GB SSD): +Output of `versioninfo()`: ``` -Julia Version 1.5.3 -Commit 788b2c77c1 (2020-11-09 13:37 UTC) +Julia Version 1.11.1 +Commit 8f5b7ca12ad (2024-10-16 10:53 UTC) +Build Info: + Official https://julialang.org/ release Platform Info: - OS: macOS (x86_64-apple-darwin18.7.0) - CPU: Intel(R) Core(TM) i7-8850H CPU @ 2.60GHz + OS: macOS (arm64-apple-darwin22.4.0) + CPU: 8 × Apple M2 WORD_SIZE: 64 - LIBM: libopenlibm - LLVM: libLLVM-9.0.1 (ORCJIT, skylake) + LLVM: libLLVM-16.0.6 (ORCJIT, apple-m2) +Threads: 1 default, 0 interactive, 1 GC (on 4 virtual cores) ``` -Note that this is a different machine than the one that was used for earlier benchmarks. - -Mass matrix: +> **Note** +> This is a different machine than the one that was used for earlier benchmarks. +Mass matrix ([`mass_matrix!`](@ref)): ``` - memory estimate: 0 bytes - allocs estimate: 0 - -------------- - minimum time: 4.415 μs (0.00% GC) - median time: 4.579 μs (0.00% GC) - mean time: 4.916 μs (0.00% GC) - maximum time: 19.794 μs (0.00% GC) +BenchmarkTools.Trial: 10000 samples with 10 evaluations. + Range (min … max): 3.728 μs … 10.024 μs ┊ GC (min … max): 0.00% … 0.00% + Time (median): 3.874 μs ┊ GC (median): 0.00% + Time (mean ± σ): 3.903 μs ± 180.208 ns ┊ GC (mean ± σ): 0.00% ± 0.00% + Memory estimate: 0 bytes, allocs estimate: 0. ``` -Mass matrix and Jacobian from left hand to right foot: - +Mass matrix ([`mass_matrix!`](@ref)) and Jacobian ([`geometric_jacobian!`](@ref)) from left hand to right foot: ``` - memory estimate: 0 bytes - allocs estimate: 0 - -------------- - minimum time: 4.860 μs (0.00% GC) - median time: 4.982 μs (0.00% GC) - mean time: 5.399 μs (0.00% GC) - maximum time: 24.712 μs (0.00% GC) +BenchmarkTools.Trial: 10000 samples with 10 evaluations. + Range (min … max): 3.941 μs … 9.020 μs ┊ GC (min … max): 0.00% … 0.00% + Time (median): 4.103 μs ┊ GC (median): 0.00% + Time (mean ± σ): 4.135 μs ± 196.842 ns ┊ GC (mean ± σ): 0.00% ± 0.00% + Memory estimate: 0 bytes, allocs estimate: 0. ``` Note the low additional cost of computing a Jacobian when the mass matrix is already computed. This is because RigidBodyDynamics.jl caches intermediate computation results. -Inverse dynamics: - +Inverse dynamics ([`inverse_dynamics!`](@ref)): ``` - memory estimate: 0 bytes - allocs estimate: 0 - -------------- - minimum time: 4.256 μs (0.00% GC) - median time: 4.541 μs (0.00% GC) - mean time: 4.831 μs (0.00% GC) - maximum time: 21.625 μs (0.00% GC) +BenchmarkTools.Trial: 10000 samples with 10 evaluations. + Range (min … max): 2.736 μs … 5.666 μs ┊ GC (min … max): 0.00% … 0.00% + Time (median): 2.866 μs ┊ GC (median): 0.00% + Time (mean ± σ): 2.882 μs ± 119.781 ns ┊ GC (mean ± σ): 0.00% ± 0.00% + Memory estimate: 0 bytes, allocs estimate: 0. ``` -Forward dynamics: - +Forward dynamics ([`dynamics!`](@ref)): ``` - memory estimate: 0 bytes - allocs estimate: 0 - -------------- - minimum time: 13.600 μs (0.00% GC) - median time: 14.419 μs (0.00% GC) - mean time: 16.071 μs (0.00% GC) - maximum time: 55.328 μs (0.00% GC) +BenchmarkTools.Trial: 10000 samples with 10 evaluations. + Range (min … max): 9.791 μs … 13.899 μs ┊ GC (min … max): 0.00% … 0.00% + Time (median): 9.874 μs ┊ GC (median): 0.00% + Time (mean ± σ): 9.942 μs ± 292.126 ns ┊ GC (mean ± σ): 0.00% ± 0.00% + Memory estimate: 0 bytes, allocs estimate: 0. ``` diff --git a/perf/Manifest.toml b/perf/Manifest.toml index 4577004c..103739af 100644 --- a/perf/Manifest.toml +++ b/perf/Manifest.toml @@ -1,39 +1,42 @@ # This file is machine-generated - editing it directly is not advised -julia_version = "1.9.0" +julia_version = "1.11.1" manifest_format = "2.0" project_hash = "65dd92ee98603779b07a7023ff0989262d569492" [[deps.ArgTools]] uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" -version = "1.1.1" +version = "1.1.2" [[deps.Artifacts]] uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" +version = "1.11.0" [[deps.Base64]] uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" +version = "1.11.0" [[deps.BenchmarkTools]] deps = ["JSON", "Logging", "Printf", "Profile", "Statistics", "UUIDs"] -git-tree-sha1 = "d9a9701b899b30332bbcb3e1679c41cce81fb0e8" +git-tree-sha1 = "f1dff6729bc61f4d49e140da1af55dcd1ac97b2f" uuid = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" -version = "1.3.2" +version = "1.5.0" [[deps.Calculus]] deps = ["LinearAlgebra"] -git-tree-sha1 = "f641eb0a4f00c343bbc32346e1217b86f3ce9dad" +git-tree-sha1 = "9cb23bbb1127eefb022b022481466c0f1127d430" uuid = "49dc2e85-a5d0-5ad3-a950-438e2897f1b9" -version = "0.5.1" +version = "0.5.2" [[deps.CompilerSupportLibraries_jll]] deps = ["Artifacts", "Libdl"] uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" -version = "1.0.2+0" +version = "1.1.1+0" [[deps.Dates]] deps = ["Printf"] uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" +version = "1.11.0" [[deps.DocStringExtensions]] deps = ["LibGit2"] @@ -54,10 +57,12 @@ version = "0.6.8" [[deps.FileWatching]] uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee" +version = "1.11.0" [[deps.InteractiveUtils]] deps = ["Markdown"] uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" +version = "1.11.0" [[deps.IrrationalConstants]] git-tree-sha1 = "630b497eafcc20001bba38a4651b327dcfc491d2" @@ -65,10 +70,10 @@ uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" version = "0.2.2" [[deps.JLLWrappers]] -deps = ["Preferences"] -git-tree-sha1 = "abc9885a7ca2052a736a600f7fa66209f96506e1" +deps = ["Artifacts", "Preferences"] +git-tree-sha1 = "be3dc50a92e5a386872a493a10050136d4703f9b" uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" -version = "1.4.1" +version = "1.6.1" [[deps.JSON]] deps = ["Dates", "Mmap", "Parsers", "Unicode"] @@ -79,46 +84,54 @@ version = "0.21.4" [[deps.LibCURL]] deps = ["LibCURL_jll", "MozillaCACerts_jll"] uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" -version = "0.6.3" +version = "0.6.4" [[deps.LibCURL_jll]] deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" -version = "7.84.0+0" +version = "8.6.0+0" [[deps.LibGit2]] -deps = ["Base64", "NetworkOptions", "Printf", "SHA"] +deps = ["Base64", "LibGit2_jll", "NetworkOptions", "Printf", "SHA"] uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" +version = "1.11.0" + +[[deps.LibGit2_jll]] +deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll"] +uuid = "e37daf67-58a4-590a-8e99-b0245dd2ffc5" +version = "1.7.2+0" [[deps.LibSSH2_jll]] deps = ["Artifacts", "Libdl", "MbedTLS_jll"] uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" -version = "1.10.2+0" +version = "1.11.0+1" [[deps.Libdl]] uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" +version = "1.11.0" [[deps.Libiconv_jll]] -deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] -git-tree-sha1 = "c7cb1f5d892775ba13767a87c7ada0b980ea0a71" +deps = ["Artifacts", "JLLWrappers", "Libdl"] +git-tree-sha1 = "61dfdba58e585066d8bce214c5a51eaa0539f269" uuid = "94ce4f54-9a6c-5748-9c1c-f9c7231a4531" -version = "1.16.1+2" +version = "1.17.0+1" [[deps.LightXML]] deps = ["Libdl", "XML2_jll"] -git-tree-sha1 = "e129d9391168c677cd4800f5c0abb1ed8cb3794f" +git-tree-sha1 = "3a994404d3f6709610701c7dabfc03fed87a81f8" uuid = "9c8b4983-aa76-5018-a973-4c85ecc9e179" -version = "0.9.0" +version = "0.9.1" [[deps.LinearAlgebra]] deps = ["Libdl", "OpenBLAS_jll", "libblastrampoline_jll"] uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +version = "1.11.0" [[deps.LogExpFunctions]] deps = ["DocStringExtensions", "IrrationalConstants", "LinearAlgebra"] -git-tree-sha1 = "0a1b7c2863e44523180fdb3146534e265a91870b" +git-tree-sha1 = "a2d09619db4e765091ee5c6ffe8872849de0feea" uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" -version = "0.3.23" +version = "0.3.28" [deps.LogExpFunctions.extensions] LogExpFunctionsChainRulesCoreExt = "ChainRulesCore" @@ -132,6 +145,7 @@ version = "0.3.23" [[deps.Logging]] uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" +version = "1.11.0" [[deps.LoopThrottle]] deps = ["Test"] @@ -142,18 +156,20 @@ version = "0.1.0" [[deps.Markdown]] deps = ["Base64"] uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" +version = "1.11.0" [[deps.MbedTLS_jll]] deps = ["Artifacts", "Libdl"] uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" -version = "2.28.2+0" +version = "2.28.6+0" [[deps.Mmap]] uuid = "a63ad114-7e13-5084-954f-fe012c677804" +version = "1.11.0" [[deps.MozillaCACerts_jll]] uuid = "14a3606d-f60d-562e-9121-12d972cd8159" -version = "2022.10.11" +version = "2023.12.12" [[deps.NaNMath]] deps = ["OpenLibm_jll"] @@ -168,12 +184,12 @@ version = "1.2.0" [[deps.OpenBLAS_jll]] deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"] uuid = "4536629a-c528-5b80-bd46-f80d51c5b363" -version = "0.3.21+4" +version = "0.3.27+1" [[deps.OpenLibm_jll]] deps = ["Artifacts", "Libdl"] uuid = "05823500-19ac-5b8b-9628-191a04bc5112" -version = "0.8.1+0" +version = "0.8.1+2" [[deps.OpenSpecFun_jll]] deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] @@ -183,34 +199,41 @@ version = "0.5.5+0" [[deps.Parsers]] deps = ["Dates", "PrecompileTools", "UUIDs"] -git-tree-sha1 = "7302075e5e06da7d000d9bfa055013e3e85578ca" +git-tree-sha1 = "8489905bcdbcfac64d1daa51ca07c0d8f0283821" uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0" -version = "2.5.9" +version = "2.8.1" [[deps.Pkg]] -deps = ["Artifacts", "Dates", "Downloads", "FileWatching", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] +deps = ["Artifacts", "Dates", "Downloads", "FileWatching", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "Random", "SHA", "TOML", "Tar", "UUIDs", "p7zip_jll"] uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" -version = "1.9.0" +version = "1.11.0" + + [deps.Pkg.extensions] + REPLExt = "REPL" + + [deps.Pkg.weakdeps] + REPL = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" [[deps.PrecompileTools]] deps = ["Preferences"] -git-tree-sha1 = "259e206946c293698122f63e2b513a7c99a244e8" +git-tree-sha1 = "5aa36f7049a63a1528fe8f7c3f2113413ffd4e1f" uuid = "aea7be01-6a6a-4083-8856-8a6e6704d82a" -version = "1.1.1" +version = "1.2.1" [[deps.Preferences]] deps = ["TOML"] -git-tree-sha1 = "7eb1686b4f04b82f96ed7a4ea5890a4f0c7a09f1" +git-tree-sha1 = "9306f6085165d270f7e3db02af26a400d580f5c6" uuid = "21216c6a-2e73-6563-6e65-726566657250" -version = "1.4.0" +version = "1.4.3" [[deps.Printf]] deps = ["Unicode"] uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" +version = "1.11.0" [[deps.Profile]] -deps = ["Printf"] uuid = "9abbd945-dff8-562f-b5e8-e1ebf5ef1b79" +version = "1.11.0" [[deps.Quaternions]] deps = ["DualNumbers", "LinearAlgebra", "Random"] @@ -218,13 +241,10 @@ git-tree-sha1 = "0b345302b17b0e694092621915de0e0dc7443a1a" uuid = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" version = "0.4.9" -[[deps.REPL]] -deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] -uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" - [[deps.Random]] -deps = ["SHA", "Serialization"] +deps = ["SHA"] uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" +version = "1.11.0" [[deps.Reexport]] git-tree-sha1 = "45e428421666073eab6f2da5c9d310d99bb12f9b" @@ -233,9 +253,9 @@ version = "1.2.2" [[deps.RigidBodyDynamics]] deps = ["DocStringExtensions", "LightXML", "LinearAlgebra", "LoopThrottle", "Random", "Reexport", "Rotations", "SparseArrays", "StaticArrays", "TypeSortedCollections", "UnsafeArrays"] -path = ".." +git-tree-sha1 = "313ea256ced33aa6039987c9fef57e59aa229589" uuid = "366cf18f-59d5-5db9-a4de-86a9f6786172" -version = "2.3.2" +version = "2.4.0" [[deps.Rotations]] deps = ["LinearAlgebra", "Quaternions", "Random", "StaticArrays", "Statistics"] @@ -249,19 +269,18 @@ version = "0.7.0" [[deps.Serialization]] uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" - -[[deps.Sockets]] -uuid = "6462fe0b-24de-5631-8697-dd941f90decc" +version = "1.11.0" [[deps.SparseArrays]] deps = ["Libdl", "LinearAlgebra", "Random", "Serialization", "SuiteSparse_jll"] uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" +version = "1.11.0" [[deps.SpecialFunctions]] deps = ["IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] -git-tree-sha1 = "ef28127915f4229c971eb43f3fc075dd3fe91880" +git-tree-sha1 = "2f5d4697f21388cbe1ff299430dd169ef97d7e14" uuid = "276daf66-3868-5448-9aa4-cd146d93841b" -version = "2.2.0" +version = "2.4.0" [deps.SpecialFunctions.extensions] SpecialFunctionsChainRulesCoreExt = "ChainRulesCore" @@ -270,25 +289,38 @@ version = "2.2.0" ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" [[deps.StaticArrays]] -deps = ["LinearAlgebra", "Random", "StaticArraysCore", "Statistics"] -git-tree-sha1 = "c262c8e978048c2b095be1672c9bee55b4619521" +deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore"] +git-tree-sha1 = "777657803913ffc7e8cc20f0fd04b634f871af8f" uuid = "90137ffa-7385-5640-81b9-e52037218182" -version = "1.5.24" +version = "1.9.8" + + [deps.StaticArrays.extensions] + StaticArraysChainRulesCoreExt = "ChainRulesCore" + StaticArraysStatisticsExt = "Statistics" + + [deps.StaticArrays.weakdeps] + ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" + Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" [[deps.StaticArraysCore]] -git-tree-sha1 = "6b7ba252635a5eff6a0b0664a41ee140a1c9e72a" +git-tree-sha1 = "192954ef1208c7019899fbf8049e717f92959682" uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" -version = "1.4.0" +version = "1.4.3" [[deps.Statistics]] -deps = ["LinearAlgebra", "SparseArrays"] +deps = ["LinearAlgebra"] +git-tree-sha1 = "ae3bb1eb3bba077cd276bc5cfc337cc65c3075c0" uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" -version = "1.9.0" +version = "1.11.1" +weakdeps = ["SparseArrays"] + + [deps.Statistics.extensions] + SparseArraysExt = ["SparseArrays"] [[deps.SuiteSparse_jll]] -deps = ["Artifacts", "Libdl", "Pkg", "libblastrampoline_jll"] +deps = ["Artifacts", "Libdl", "libblastrampoline_jll"] uuid = "bea87d4a-7f5b-5778-9afe-8cc45184846c" -version = "5.10.1+6" +version = "7.7.0+0" [[deps.TOML]] deps = ["Dates"] @@ -303,6 +335,7 @@ version = "1.10.0" [[deps.Test]] deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" +version = "1.11.0" [[deps.TypeSortedCollections]] git-tree-sha1 = "d539b357e7695d80c75f5b066cec5d8c45886ab2" @@ -312,37 +345,39 @@ version = "1.1.0" [[deps.UUIDs]] deps = ["Random", "SHA"] uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" +version = "1.11.0" [[deps.Unicode]] uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" +version = "1.11.0" [[deps.UnsafeArrays]] -git-tree-sha1 = "3350f94f6caa02f324a23645bf524fc9334c7488" +git-tree-sha1 = "da0c9ca60d3371a4bc86b4e65c45db17086fb3ac" uuid = "c4a57d5a-5b31-53a6-b365-19f8c011fbd6" -version = "1.0.4" +version = "1.0.6" [[deps.XML2_jll]] -deps = ["Artifacts", "JLLWrappers", "Libdl", "Libiconv_jll", "Pkg", "Zlib_jll"] -git-tree-sha1 = "93c41695bc1c08c46c5899f4fe06d6ead504bb73" +deps = ["Artifacts", "JLLWrappers", "Libdl", "Libiconv_jll", "Zlib_jll"] +git-tree-sha1 = "6a451c6f33a176150f315726eba8b92fbfdb9ae7" uuid = "02c8fc9c-b97f-50b9-bbe4-9be30ff0a78a" -version = "2.10.3+0" +version = "2.13.4+0" [[deps.Zlib_jll]] deps = ["Libdl"] uuid = "83775a58-1f1d-513f-b197-d71354ab007a" -version = "1.2.13+0" +version = "1.2.13+1" [[deps.libblastrampoline_jll]] deps = ["Artifacts", "Libdl"] uuid = "8e850b90-86db-534c-a0d3-1478176c7d93" -version = "5.7.0+0" +version = "5.11.0+0" [[deps.nghttp2_jll]] deps = ["Artifacts", "Libdl"] uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" -version = "1.48.0+0" +version = "1.59.0+0" [[deps.p7zip_jll]] deps = ["Artifacts", "Libdl"] uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" -version = "17.4.0+0" +version = "17.4.0+2" diff --git a/perf/runbenchmarks.jl b/perf/runbenchmarks.jl index d3681896..0d44c25b 100644 --- a/perf/runbenchmarks.jl +++ b/perf/runbenchmarks.jl @@ -7,10 +7,6 @@ using RigidBodyDynamics.OdeIntegrators using Random using Profile using BenchmarkTools -using LinearAlgebra - -# Due to https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/500: -BLAS.set_num_threads(1) const ScalarType = Float64 # const ScalarType = Float32 diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index 96ff7017..70710b17 100644 --- a/src/RigidBodyDynamics.jl +++ b/src/RigidBodyDynamics.jl @@ -21,7 +21,8 @@ export DynamicsResult, StateCache, DynamicsResultCache, - SegmentedVectorCache + SegmentedVectorCache, + WrenchesCache # specific joint types export diff --git a/src/caches.jl b/src/caches.jl index e13e6f2c..7fcfa64d 100644 --- a/src/caches.jl +++ b/src/caches.jl @@ -107,3 +107,28 @@ end @inline function makevalue(c::SegmentedVectorCache{K, KeyRange}, ::Type{T}) where {K, T, KeyRange} SegmentedVector{K, T, KeyRange}(Vector{T}(undef, c.length), c.ranges) end + +""" +$(TYPEDEF) + +A container that manages the creation and storage of `BodyDict{Wrench{T}}`. +Similar to [`StateCache`](@ref). +""" +struct WrenchesCache{M} <: AbstractTypeDict + mechanism::Mechanism{M} + keys::Vector{UInt} + values::Vector # Can also be `Any` +end + +Base.show(io::IO, ::WrenchesCache{M}) where {M} = print(io, "WrenchesCache{$M}(…)") + +WrenchesCache(mechanism::Mechanism{M}) where {M} = WrenchesCache{M}(mechanism, [], []) + +@inline function valuetype(::Type{WrenchesCache{M}}, ::Type{T}) where {M,T} + BodyDict{Wrench{T}} +end + +@inline function makevalue(c::WrenchesCache, ::Type{T}) where {T} + rootframe = root_frame(c.mechanism) + BodyDict{Wrench{T}}(b => zero(Wrench{T}, rootframe) for b in bodies(c.mechanism)) +end diff --git a/src/spatial/threevectors.jl b/src/spatial/threevectors.jl index d8eae288..213bb375 100644 --- a/src/spatial/threevectors.jl +++ b/src/spatial/threevectors.jl @@ -75,7 +75,7 @@ LinearAlgebra.cross(v1::FreeVector3D, v2::FreeVector3D) = begin @framecheck(v1.f LinearAlgebra.dot(v1::FreeVector3D, v2::FreeVector3D) = begin @framecheck(v1.frame, v2.frame); v1.v ⋅ v2.v end Base.:*(t::Transform3D, vector::FreeVector3D) = begin @framecheck(t.from, vector.frame); FreeVector3D(t.to, rotation(t) * vector.v) end Base.:\(t::Transform3D, point::FreeVector3D) = begin @framecheck point.frame t.to; FreeVector3D(t.from, rotation(t) \ point.v) end -LinearAlgebra.norm(v::FreeVector3D) = norm(v.v) +LinearAlgebra.norm(v::FreeVector3D) = LinearAlgebra.generic_norm2(v.v) LinearAlgebra.normalize(v::FreeVector3D, p::Real = 2) = FreeVector3D(v.frame, normalize(v.v, p)) # Mixed Point3D and FreeVector3D