From 8a68607451094f336f92eb1970ef6dd4583fba05 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 12 Jun 2024 15:33:06 +0200 Subject: [PATCH] quaternions in Spherical --- Manifest.toml | 83 +++++++++++++++--------------- src/joints.jl | 49 ++++++++++-------- src/orientation.jl | 7 ++- test/runtests.jl | 125 ++++++++++++++++++++++++++++++++++++++++++++- 4 files changed, 198 insertions(+), 66 deletions(-) diff --git a/Manifest.toml b/Manifest.toml index af90b200..36d1526e 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -1,13 +1,13 @@ # This file is machine-generated - editing it directly is not advised -julia_version = "1.10.3" +julia_version = "1.10.4" manifest_format = "2.0" project_hash = "efeef262c86b4eafbecc9a61dea3d204d89a22d1" [[deps.ADTypes]] -git-tree-sha1 = "daf26bbdec60d9ca1c0003b70f389d821ddb4224" +git-tree-sha1 = "fc02d55798c1af91123d07915a990fbb9a10d146" uuid = "47edcb42-4c32-4615-8424-f2b9edc5f35b" -version = "1.2.1" +version = "1.3.0" weakdeps = ["ChainRulesCore", "EnzymeCore"] [deps.ADTypes.extensions] @@ -68,9 +68,9 @@ version = "0.4.0" [[deps.ArrayInterface]] deps = ["Adapt", "LinearAlgebra", "SparseArrays", "SuiteSparse"] -git-tree-sha1 = "133a240faec6e074e07c31ee75619c90544179cf" +git-tree-sha1 = "ed2ec3c9b483842ae59cd273834e5b46206d6dda" uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" -version = "7.10.0" +version = "7.11.0" [deps.ArrayInterface.extensions] ArrayInterfaceBandedMatricesExt = "BandedMatrices" @@ -96,9 +96,9 @@ version = "7.10.0" [[deps.ArrayLayouts]] deps = ["FillArrays", "LinearAlgebra"] -git-tree-sha1 = "29649b61e0313db0a7ad5ecf41210e4e85aea234" +git-tree-sha1 = "420e2853770f50e5306b9d96b5a66f26e7c73bc6" uuid = "4c555306-a7a7-4459-81d9-ec55ddd5c99a" -version = "1.9.3" +version = "1.9.4" weakdeps = ["SparseArrays"] [deps.ArrayLayouts.extensions] @@ -146,9 +146,9 @@ version = "0.5.1" [[deps.ChainRules]] deps = ["Adapt", "ChainRulesCore", "Compat", "Distributed", "GPUArraysCore", "IrrationalConstants", "LinearAlgebra", "Random", "RealDot", "SparseArrays", "SparseInverseSubset", "Statistics", "StructArrays", "SuiteSparse"] -git-tree-sha1 = "5ec157747036038ec70b250f578362268f0472f1" +git-tree-sha1 = "227985d885b4dbce5e18a96f9326ea1e836e5a03" uuid = "082447d4-558c-5d27-93f4-14fc19e9eca2" -version = "1.68.0" +version = "1.69.0" [[deps.ChainRulesCore]] deps = ["Compat", "LinearAlgebra"] @@ -344,9 +344,9 @@ version = "1.15.1" [[deps.DifferentiationInterface]] deps = ["ADTypes", "Compat", "DocStringExtensions", "FillArrays", "LinearAlgebra", "PackageExtensionCompat", "SparseArrays", "SparseMatrixColorings"] -git-tree-sha1 = "b4ce591b91f500126c9c5030aa5b8a710c7db7a0" +git-tree-sha1 = "23c6df13ad8fcffde4b0596d798911d2e309fc2c" uuid = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63" -version = "0.4.2" +version = "0.5.5" [deps.DifferentiationInterface.extensions] DifferentiationInterfaceChainRulesCoreExt = "ChainRulesCore" @@ -361,7 +361,7 @@ version = "0.4.2" DifferentiationInterfaceSymbolicsExt = "Symbolics" DifferentiationInterfaceTapirExt = "Tapir" DifferentiationInterfaceTrackerExt = "Tracker" - DifferentiationInterfaceZygoteExt = "Zygote" + DifferentiationInterfaceZygoteExt = ["Zygote", "ForwardDiff"] [deps.DifferentiationInterface.weakdeps] ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" @@ -468,9 +468,9 @@ uuid = "4e289a0a-7415-4d19-859d-a7e5c4648b56" version = "1.0.4" [[deps.EnzymeCore]] -git-tree-sha1 = "0910982db2490a20f81dc7db5d4bbea236c027b3" +git-tree-sha1 = "a2f4588bde1588af6279729540099895f8dee843" uuid = "f151be2c-9106-41f4-ab19-57ee4f262869" -version = "0.7.3" +version = "0.7.4" weakdeps = ["Adapt"] [deps.EnzymeCore.extensions] @@ -495,9 +495,9 @@ version = "0.8.5" [[deps.FastBroadcast]] deps = ["ArrayInterface", "LinearAlgebra", "Polyester", "Static", "StaticArrayInterface", "StrideArraysCore"] -git-tree-sha1 = "e17367f052035620d832499496080f792fa7ea47" +git-tree-sha1 = "2be93e36303143c6fffd07e2222bbade35194d9e" uuid = "7034ab61-46d4-4ed7-9d0f-46aef9175898" -version = "0.3.2" +version = "0.3.3" [[deps.FastClosures]] git-tree-sha1 = "acebe244d53ee1b461970f8910c235b259e772ef" @@ -607,9 +607,9 @@ version = "1.3.1" [[deps.Graphs]] deps = ["ArnoldiMethod", "Compat", "DataStructures", "Distributed", "Inflate", "LinearAlgebra", "Random", "SharedArrays", "SimpleTraits", "SparseArrays", "Statistics"] -git-tree-sha1 = "4f2b57488ac7ee16124396de4f2bbdd51b2602ad" +git-tree-sha1 = "334d300809ae0a68ceee3444c6e99ded412bf0b3" uuid = "86223c79-3864-5bf0-83f7-82e725a168b6" -version = "1.11.0" +version = "1.11.1" [[deps.HostCPUFeatures]] deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Static"] @@ -693,10 +693,10 @@ uuid = "98e50ef6-434e-11e9-1051-2b60c6c9e899" version = "1.0.56" [[deps.JuliaSimBase]] -deps = ["Preferences"] -git-tree-sha1 = "a2cce509f83f17761da1a5963e7bf807d020b05a" +deps = ["ModelingToolkit", "OrdinaryDiffEq", "Preferences", "SciMLBase", "UUIDs"] +git-tree-sha1 = "5b989620db9be15cdd70b58f48f5cdbcf54364ea" uuid = "9c9cc66b-a38b-4ec4-8b59-87b54775939f" -version = "0.1.0" +version = "1.1.0" [[deps.JuliaSimCompiler]] deps = ["AbstractTrees", "ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "GPUCompiler", "Graphs", "IfElse", "JuliaFormatter", "JuliaSimBase", "JuliaSimCompilerRuntime", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "NaNMath", "OffsetArrays", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SnoopPrecompile", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StrideArraysCore", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "UnPack"] @@ -983,11 +983,11 @@ version = "9.15.0" [[deps.ModelingToolkitStandardLibrary]] deps = ["ChainRulesCore", "DiffEqBase", "IfElse", "LinearAlgebra", "ModelingToolkit", "Symbolics"] -git-tree-sha1 = "d2c67a4fde6f3ca0e18b603c918855862e7c0e0d" -repo-rev = "baggepinnen-patch-1" +git-tree-sha1 = "e36487496e263d82e866ce5f9d3e87f44765e6ea" +repo-rev = "main" repo-url = "https://github.com/SciML/ModelingToolkitStandardLibrary.jl.git" uuid = "16a59e39-deab-5bd0-87e4-056b12336739" -version = "2.7.1" +version = "2.7.2" [[deps.MozillaCACerts_jll]] uuid = "14a3606d-f60d-562e-9121-12d972cd8159" @@ -1096,9 +1096,9 @@ version = "1.6.3" [[deps.OrdinaryDiffEq]] deps = ["ADTypes", "Adapt", "ArrayInterface", "DataStructures", "DiffEqBase", "DocStringExtensions", "EnumX", "ExponentialUtilities", "FastBroadcast", "FastClosures", "FillArrays", "FiniteDiff", "ForwardDiff", "FunctionWrappersWrappers", "IfElse", "InteractiveUtils", "LineSearches", "LinearAlgebra", "LinearSolve", "Logging", "MacroTools", "MuladdMacro", "NonlinearSolve", "Polyester", "PreallocationTools", "PrecompileTools", "Preferences", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "SciMLStructures", "SimpleNonlinearSolve", "SimpleUnPack", "SparseArrays", "SparseDiffTools", "StaticArrayInterface", "StaticArrays", "TruncatedStacktraces"] -git-tree-sha1 = "75b0d2bf28d0df92931919004a5be5304c38cca2" +git-tree-sha1 = "b857d515bacfb46ee3603a27d6d15b196cf285d0" uuid = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" -version = "6.80.1" +version = "6.82.0" [[deps.PDMats]] deps = ["LinearAlgebra", "SparseArrays", "SuiteSparse"] @@ -1226,9 +1226,9 @@ version = "1.3.4" [[deps.RecursiveArrayTools]] deps = ["Adapt", "ArrayInterface", "DocStringExtensions", "GPUArraysCore", "IteratorInterfaceExtensions", "LinearAlgebra", "RecipesBase", "SparseArrays", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"] -git-tree-sha1 = "2cea01606a852c2431ded77293eb533b511b19e6" +git-tree-sha1 = "3400ce27995422fb88ffcd3af9945565aad947f0" uuid = "731186ca-8d62-57ce-b412-fbd966d074cd" -version = "3.22.0" +version = "3.23.1" [deps.RecursiveArrayTools.extensions] RecursiveArrayToolsFastBroadcastExt = "FastBroadcast" @@ -1309,10 +1309,10 @@ uuid = "476501e8-09a2-5ece-8869-fb82de89a1fa" version = "0.6.42" [[deps.SciMLBase]] -deps = ["ADTypes", "ArrayInterface", "CommonSolve", "ConstructionBase", "Distributed", "DocStringExtensions", "EnumX", "FunctionWrappersWrappers", "IteratorInterfaceExtensions", "LinearAlgebra", "Logging", "Markdown", "PrecompileTools", "Preferences", "Printf", "RecipesBase", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLOperators", "SciMLStructures", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"] -git-tree-sha1 = "9f59654e2a85017ee27b0f59c7fac5a57aa10ced" +deps = ["ADTypes", "Accessors", "ArrayInterface", "CommonSolve", "ConstructionBase", "Distributed", "DocStringExtensions", "EnumX", "FunctionWrappersWrappers", "IteratorInterfaceExtensions", "LinearAlgebra", "Logging", "Markdown", "PrecompileTools", "Preferences", "Printf", "RecipesBase", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLOperators", "SciMLStructures", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"] +git-tree-sha1 = "c15e03738d4170f92ba477273ef0528341f79a9a" uuid = "0bca4576-84f4-4d90-8ffe-ffa030f20462" -version = "2.39.0" +version = "2.41.0" [deps.SciMLBase.extensions] SciMLBaseChainRulesCoreExt = "ChainRulesCore" @@ -1340,9 +1340,10 @@ uuid = "c0aeaf25-5076-4817-a8d5-81caf7dfa961" version = "0.3.8" [[deps.SciMLStructures]] -git-tree-sha1 = "d778a74df2f64059c38453b34abad1953b2b8722" +deps = ["ArrayInterface"] +git-tree-sha1 = "6ab4beaf88dcdd2639bead916f2347f81dcacd0e" uuid = "53ae85a6-f571-4167-b2af-e1d143709226" -version = "1.2.0" +version = "1.3.0" [[deps.Scratch]] deps = ["Dates"] @@ -1365,9 +1366,9 @@ uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" [[deps.SimpleNonlinearSolve]] deps = ["ADTypes", "ArrayInterface", "ConcreteStructs", "DiffEqBase", "DiffResults", "DifferentiationInterface", "FastClosures", "FiniteDiff", "ForwardDiff", "LinearAlgebra", "MaybeInplace", "PrecompileTools", "Reexport", "SciMLBase", "Setfield", "StaticArraysCore"] -git-tree-sha1 = "f3c50acd5145f2c6ee85343ce6f433dd2caab41e" +git-tree-sha1 = "913754ccbbc78720a4542b56a6bdfbab1c84c8f2" uuid = "727e6d20-b764-4bd8-a329-72de5adea6c7" -version = "1.9.0" +version = "1.10.0" [deps.SimpleNonlinearSolve.extensions] SimpleNonlinearSolveChainRulesCoreExt = "ChainRulesCore" @@ -1440,9 +1441,9 @@ version = "0.1.2" [[deps.SparseMatrixColorings]] deps = ["ADTypes", "Compat", "DocStringExtensions", "LinearAlgebra", "Random", "SparseArrays"] -git-tree-sha1 = "d4adedbcc8776c567e0e22ef19f13cf10cb6ecaa" +git-tree-sha1 = "eed2446b3c3dd58f6ded3168998b8b2cb3fc9229" uuid = "0a514795-09f3-496d-8182-132a7b665d35" -version = "0.3.2" +version = "0.3.3" [[deps.Sparspak]] deps = ["Libdl", "LinearAlgebra", "Logging", "OffsetArrays", "Printf", "SparseArrays", "Test"] @@ -1479,9 +1480,9 @@ weakdeps = ["OffsetArrays", "StaticArrays"] [[deps.StaticArrays]] deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore"] -git-tree-sha1 = "9ae599cd7529cfce7fea36cf00a62cfc56f0f37c" +git-tree-sha1 = "6e00379a24597be4ae1ee6b2d882e15392040132" uuid = "90137ffa-7385-5640-81b9-e52037218182" -version = "1.9.4" +version = "1.9.5" weakdeps = ["ChainRulesCore", "Statistics"] [deps.StaticArrays.extensions] @@ -1489,9 +1490,9 @@ weakdeps = ["ChainRulesCore", "Statistics"] StaticArraysStatisticsExt = "Statistics" [[deps.StaticArraysCore]] -git-tree-sha1 = "36b3d696ce6366023a0ea192b4cd442268995a0d" +git-tree-sha1 = "192954ef1208c7019899fbf8049e717f92959682" uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" -version = "1.4.2" +version = "1.4.3" [[deps.Statistics]] deps = ["LinearAlgebra", "SparseArrays"] diff --git a/src/joints.jl b/src/joints.jl index 51a9e5d8..6c3a8b55 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -167,7 +167,9 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin z_rel_a_fixed = false, sequence_angleStates = [1, 2, 3], phi = 0, phi_d = 0, d = 0, - phi_dd = 0) + phi_dd = 0, + useQuaternions = false, + ) @named begin ptf = PartialTwoFrames() R_rel = NumRotationMatrix() @@ -195,19 +197,24 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin end if enforceState - @variables begin - (phi(t)[1:3] = phi), - [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"] - (phi_d(t)[1:3] = phi_d), - [state_priority = 10, description = "3 angle derivatives"] - (phi_dd(t)[1:3] = phi_dd), - [state_priority = 10, description = "3 angle second derivatives"] + if useQuaternions + append!(eqs, nonunit_quaternion_equations(R_rel, w_rel)) + # append!(eqs, collect(w_rel) .~ angularVelocity2(R_rel)) + else + @variables begin + (phi(t)[1:3] = phi), + [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"] + (phi_d(t)[1:3] = phi_d), + [state_priority = 10, description = "3 angle derivatives"] + (phi_dd(t)[1:3] = phi_dd), + [state_priority = 10, description = "3 angle second derivatives"] + end + append!(eqs, + [R_rel ~ axesRotations(sequence_angleStates, phi, phi_d) + collect(w_rel) .~ angularVelocity2(R_rel) + collect(phi_d .~ D.(phi)) + collect(phi_dd .~ D.(phi_d))]) end - append!(eqs, - [R_rel ~ axesRotations(sequence_angleStates, phi, phi_d) - collect(w_rel) .~ angularVelocity2(R_rel) - collect(phi_d .~ D.(phi)) - collect(phi_dd .~ D.(phi_d))]) if isroot append!(eqs, [ori(frame_b) ~ absoluteRotation(frame_a, R_rel) @@ -719,13 +726,15 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi end if useQuaternions - @named Q = NumQuaternion(varw=false) - append!(eqs, [ - w_rel_b .~ angularVelocity2(Q, D.(Q.Q)) - R_rel ~ from_Q(Q, w_rel_b) - # R_rel.w .~ w_rel_b # Needed? If not included, w_rel_b has no effect. To include, use R_rel = NumRotationMatrix(varw=true), but this yeilds the same numer of state variables - 0 ~ orientation_constraint(Q) - ]) + # @named Q = NumQuaternion() + append!(eqs, nonunit_quaternion_equations(R_rel, w_rel_b)) + + # append!(eqs, [ + # w_rel_b .~ angularVelocity2(Q, D.(Q.Q)) + # R_rel ~ from_Q(Q, w_rel_b) + # R_rel.w .~ w_rel_b; # Needed? If not included, w_rel_b has no effect. To include, use R_rel = NumRotationMatrix(varw=true), but this yeilds the same numer of state variables + # 0 ~ orientation_constraint(Q) + # ]) else append!(eqs, [phi_d .~ D.(phi) diff --git a/src/orientation.jl b/src/orientation.jl index 3cb14033..6ddf2a66 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -262,8 +262,7 @@ orientation_constraint(q::Quaternion) = orientation_constraint(q.Q) Base.:/(q::Rotations.Quaternions.Quaternion, x::Num) = Rotations.Quaternions.Quaternion(q.s / x, q.v1 / x, q.v2 / x, q.v3 / x) function from_Q(Q, w) - # TODO: shift Q elements 1,4 - Q2 = [Q[4], Q[1], Q[2], Q[3]] + Q2 = [Q[4], Q[1], Q[2], Q[3]] # Due to different conventions q = Rotations.QuatRotation(Q2) R = RotMatrix(q) RotationMatrix(R, w) @@ -425,8 +424,8 @@ end function nonunit_quaternion_equations(R, w) - @variables Q(t)[1:4]=[0,0,0,1] # normalized - @variables Q̂(t)[1:4]=[0,0,0,1] # Non-normalized + @variables Q(t)[1:4]=[0,0,0,1], [description="Unit quaternion with [i,j,k,w]"] # normalized + @variables Q̂(t)[1:4]=[0,0,0,1], [description="Non-unit quaternion with [i,j,k,w]"] # Non-normalized @variables n(t)=1 c(t)=0 @parameters k = 0.1 Q̂ = collect(Q̂) diff --git a/test/runtests.jl b/test/runtests.jl index ca93cadd..cc91f927 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -989,7 +989,7 @@ tt = 0:0.1:10 # ============================================================================== -## Simple pendulum with quaternions============================================================= +## Simple motion with quaternions============================================================= # ============================================================================== using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler t = Multibody.t @@ -1054,6 +1054,7 @@ end # Test that rotational velocity of 1 results in one full rotation in 2π seconds. Test rotation around all three major axes @test get_R(body.frame_a, 0pi) ≈ I +@test get_R(body.frame_a, pi/2) ≈ [1 0 0; 0 0 -1; 0 1 0] atol=1e-3 @test get_R(body.frame_a, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3 @test get_R(body.frame_a, 2pi) ≈ I atol=1e-3 @@ -1069,3 +1070,125 @@ sol = solve(prob, Rodas4())#, @test get_R(body.frame_a, 0pi) ≈ I @test get_R(body.frame_a, 1pi) ≈ diagm([-1, -1, 1]) atol=1e-3 @test get_R(body.frame_a, 2pi) ≈ I atol=1e-3 + + + +## states in joint instead of body + +using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler +t = Multibody.t + +@named joint = Multibody.FreeMotion(isroot = true, enforceState=true, useQuaternions=true) +@named body = Body(; m = 1, r_cm = [0.0, 0, 0]) + +world = Multibody.world + + +connections = [connect(world.frame_b, joint.frame_a) + connect(joint.frame_b, body.frame_a)] + + +@named model = ODESystem(connections, t, + systems = [world, joint, body]) +irsys = IRSystem(model) +ssys = structural_simplify(irsys) + + + +## +D = Differential(t) +# q0 = randn(4); q0 ./= norm(q0) +q0 = [0,0,0,1] +prob = ODEProblem(ssys, [ + collect(body.w_a) .=> [1,0,0]; + collect(joint.Q) .=> q0; + collect(joint.Q̂) .=> q0; + ], (0, 2pi)) + +using OrdinaryDiffEq, Test +sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 1e-6 .* randn.()) +@test SciMLBase.successful_retcode(sol) +# doplot() && plot(sol, layout=21) + + +ts = 0:0.1:2pi +Q = Matrix(sol(ts, idxs = [joint.Q...])) +Qh = Matrix(sol(ts, idxs = [joint.Q̂...])) +n = Matrix(sol(ts, idxs = [joint.n...])) +@test mapslices(norm, Qh, dims=1).^2 ≈ n +@test Q ≈ Qh ./ sqrt.(n) atol=1e-2 +@test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2 + + + +@test get_R(joint.frame_b, 0pi) ≈ I +@test get_R(joint.frame_b, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3 +@test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3 + + +Matrix(sol(ts, idxs = [joint.w_rel_b...])) + + + + + +## Spherical joint pendulum with quaternions + +using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler +t = Multibody.t +world = Multibody.world + + +@named joint = Multibody.Spherical(isroot=false, enforceState=false, useQuaternions=false) +@named rod = FixedTranslation(; r = [1, 0, 0]) +@named body = Body(; m = 1, isroot=true, useQuaternions=true, air_resistance=0.0) + +# @named joint = Multibody.Spherical(isroot=true, enforceState=true, useQuaternions=true) +# @named body = Body(; m = 1, r_cm = [1.0, 0, 0], isroot=false) + + + +connections = [connect(world.frame_b, joint.frame_a) + connect(joint.frame_b, rod.frame_a) + connect(rod.frame_b, body.frame_a)] + + +@named model = ODESystem(connections, t, + systems = [world, joint, body, rod]) +irsys = IRSystem(model) +ssys = structural_simplify(irsys) + + +D = Differential(t) +q0 = randn(4); q0 ./= norm(q0) +# q0 = [0,0,0,1] +prob = ODEProblem(ssys, [ + # collect(body.w_a) .=> [1,0,0]; + # collect(body.Q) .=> q0; + # collect(body.Q̂) .=> q0; + ], (0, 30)) + +using OrdinaryDiffEq, Test +sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 0 .* randn.()) +@test SciMLBase.successful_retcode(sol) +# doplot() && plot(sol, layout=21) + + +ts = 0:0.1:2pi +Q = Matrix(sol(ts, idxs = [body.Q...])) +Qh = Matrix(sol(ts, idxs = [body.Q̂...])) +n = Matrix(sol(ts, idxs = [body.n...])) +@test mapslices(norm, Qh, dims=1).^2 ≈ n +@test Q ≈ Qh ./ sqrt.(n) atol=1e-2 +@test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2 + +Matrix(sol(ts, idxs = [body.w_a...])) + +@test get_R(joint.frame_b, 0pi) ≈ I +@test get_R(joint.frame_b, sqrt(9.81/1)) ≈ diagm([1, -1, -1]) atol=1e-3 +@test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3 + + +Matrix(sol(ts, idxs = [joint.w_rel_b...])) + +# render(model, sol) \ No newline at end of file