quaternions in Spherical

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,19 +1480,19 @@ 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]
  StaticArraysChainRulesCoreExt = "ChainRulesCore"
  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"]

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)

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̂)