Improved state priority for quaternions (#92)

* turn of quaternion normalization

* eliminate Q variable

* eliminate `n`

* bump state priority Quat

* turn of quaternion normalization

* eliminate Q variable

* eliminate `n`

* bump state priority Quat

* clean up

* tweak test tol

* broek test fixed

* up test

Author: baggepinnen

docs/src/examples/gyroscopic_effects.md

@@ -20,7 +20,7 @@ world = Multibody.world
 
 systems = @named begin
     spherical = Spherical(state=true, radius=0.02, color=[1,1,0,1])
-    body1 = BodyCylinder(r = [0.25, 0, 0], diameter = 0.05, isroot=false, quat=false)
+    body1 = BodyCylinder(r = [0.25, 0, 0], diameter = 0.05)
     rot = FixedRotation(; n = [0,1,0], angle=deg2rad(45))
     revolute = Revolute(n = [1,0,0], radius=0.06, color=[1,0,0,1])
     trans = FixedTranslation(r = [-0.1, 0, 0])

src/components.jl

@@ -255,7 +255,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
     ]
     @variables g_0(t)[1:3] [guess = 0, description = "gravity acceleration"]
     @variables w_a(t)[1:3]=w_a [guess = 0, 
-        state_priority = 2,
+        state_priority = 2+2quat*state,
         description = "Absolute angular velocity of frame_a resolved in frame_a",
     ]
     @variables z_a(t)[1:3] [guess = 0, 

src/frames.jl

@@ -13,7 +13,7 @@
     r_0, f, tau = collect.((r_0, f, tau))
     # R: Orientation object to rotate the world frame into the connector frame
 
-    R = NumRotationMatrix(; name, varw, state_priority=0)
+    R = NumRotationMatrix(; name, varw, state_priority=-1)
 
     ODESystem(Equation[], t, [r_0; f; tau], []; name,
               metadata = Dict(:orientation => R, :frame => true))

src/orientation.jl

@@ -257,7 +257,7 @@ end
 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(Q2, w)
     # Q2 = to_q(Q) # Due to different conventions
-    q = Rotations.QuatRotation(Q2)
+    q = Rotations.QuatRotation(Q2, false)
     R = RotMatrix(q)
     RotationMatrix(R, w)
 end
@@ -404,11 +404,14 @@ function get_frame(sol, frame, t)
 end
 
 function nonunit_quaternion_equations(R, w)
-    @variables Q(t)[1:4]=[1,0,0,0], [description="Unit quaternion with [w,i,j,k]"] # normalized
-    @variables Q̂(t)[1:4]=[1,0,0,0], [description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized
+    @variables Q(t)[1:4]=[1,0,0,0], [state_priority=-1, description="Unit quaternion with [w,i,j,k]"] # normalized
+    @variables Q̂(t)[1:4]=[1,0,0,0], [state_priority=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized
+    @variables Q̂d(t)[1:4]=[0,0,0,0], [state_priority=1000]
+    # NOTE: 
     @variables n(t)=1 c(t)=0
     @parameters k = 0.1
     Q̂ = collect(Q̂)
+    Q̂d = collect(Q̂d)
     Q = collect(Q)
     # w is used in Ω, and Ω determines D(Q̂)
     # This corresponds to modelica's 
@@ -417,12 +420,13 @@ function nonunit_quaternion_equations(R, w)
     # They also have w_a = angularVelocity2(frame_a.R) even for quaternions, so w_a = angularVelocity2(Q, der(Q)), this is their link between w_a and D(Q), while ours is D(Q̂) .~ (Ω * Q̂)
     Ω = [0 -w[1] -w[2] -w[3]; w[1] 0 w[3] -w[2]; w[2] -w[3] 0 w[1]; w[3] w[2] -w[1] 0]
     # QR = from_Q(Q, angular_velocity2(Q, D.(Q)))
-    QR = from_Q(Q, w)
+    QR = from_Q(Q̂ ./ sqrt(n), w)
     [
         n ~ Q̂'Q̂
         c ~ k * (1 - n)
-        D.(Q̂) .~ (Ω' * Q̂) ./ 2 + c * Q̂ # We use Ω' which is the same as using -w to handle the fact that w coming in here is typically described frame_a rather than in frame_b, the paper is formulated with w being expressed in the rotating body frame (frame_b)
+        D.(Q̂) .~ Q̂d
+        Q̂d .~ (Ω' * Q̂) ./ 2 + c * Q̂ # We use Ω' which is the same as using -w to handle the fact that w coming in here is typically described frame_a rather than in frame_b, the paper is formulated with w being expressed in the rotating body frame (frame_b)
         Q .~ Q̂ ./ sqrt(n)
-        R ~ QR
+        R ~ from_Q(Q, w)
     ]
 end

test/runtests.jl

@@ -1036,3 +1036,71 @@ using LinearAlgebra
 end
 
 ##
+
+using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq
+using Multibody.Rotations: RotXYZ
+t = Multibody.t
+D = Multibody.D
+world = Multibody.world
+
+@named joint = Multibody.Spherical(isroot=false, state=false, quat=false)
+@named rod = FixedTranslation(; r = [1, 0, 0])
+@named body = Body(; m = 1, isroot=true, quat=true)
+
+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)
+prob = ODEProblem(ssys, [
+    # vec(ori(rod.frame_a).R) .=> vec(RotXYZ(0,0,0));
+    # D.(body.Q̂) .=> 0;
+
+], (0, 1))
+sol1 = solve(prob, Rodas4())
+
+## quat in joint
+@named joint = Multibody.Spherical(isroot=true, state=true, quat=true)
+@named rod = FixedTranslation(; r = [1, 0, 0])
+@named body = Body(; m = 1, isroot=false, quat=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)
+prob = ODEProblem(ssys, [
+    # vec(ori(rod.frame_a).R) .=> vec(RotXYZ(0,0,0));
+    # D.(joint.Q̂) .=> 0;
+
+], (0, 1))
+sol2 = solve(prob, Rodas4())
+
+## euler
+@named joint = Multibody.Spherical(isroot=true, state=true, quat=false)
+@named rod = FixedTranslation(; r = [1, 0, 0])
+@named body = Body(; m = 1, isroot=false, quat=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)
+prob = ODEProblem(ssys, [
+    # vec(ori(rod.frame_a).R) .=> vec(RotXYZ(0,0,0));
+    # D.(joint.Q̂) .=> 0;
+
+], (0, 1))
+sol3 = solve(prob, Rodas4())
+
+@test sol1(0:0.1:1, idxs=collect(body.r_0)) ≈ sol2(0:0.1:1, idxs=collect(body.r_0)) atol=1e-5
+@test sol1(0:0.1:1, idxs=collect(body.r_0)) ≈ sol3(0:0.1:1, idxs=collect(body.r_0)) atol=1e-3

test/test_orientation_getters.jl

@@ -71,7 +71,7 @@ arm_r = sol(1, idxs=collect(model.upper_arm.r))
 
 # The lower arm is finally having an extent along the $y$-axis. At the final time when the pendulum motion has been fully damped, we see that the second frame of this body ends up with an $y$-coordinate of `-0.6`:
 t1 = get_trans(sol, model.lower_arm.frame_b, 12)
-@test t1 ≈ [-0.009040487302666853, -0.59999996727278, 0.599931920189277]
+@test t1 ≈ [-0.009040487302666853, -0.59999996727278, 0.599931920189277] rtol=1e-6
 
 
 # If we rotate the vector of extent of the lower arm to the world frame, we indeed see that the only coordinate that is nonzero is the $y$ coordinate:

test/test_quaternions.jl

@@ -184,7 +184,7 @@ end
     @test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2
 
     @test get_R(sol, joint.frame_b, 0pi) ≈ I
-    @test_broken get_R(sol, joint.frame_b, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3
+    @test get_R(sol, joint.frame_b, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3
     @test get_R(sol, joint.frame_b, 2pi) ≈ I atol=1e-3
 
     Matrix(sol(ts, idxs = [joint.w_rel_b...]))
@@ -301,8 +301,8 @@ using Multibody.Rotations: params
                         collect(body.body.w_a .=> 0);
                     collect(body.body.v_0 .=> 0);
                     #   collect(body.body.phi .=> deg2rad(10));
-                        collect(body.body.Q) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
-                        collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+                        collect(body.body.Q) .=> params(Quat(RotXYZ(deg2rad.((10,10,10))...)));
+                        collect(body.body.Q̂) .=> params(Quat(RotXYZ(deg2rad.((10,10,10))...)));
                     ], (0, 10))
 
     # @test_skip begin # The modelica example uses angles_fixed = true, which causes the body component to run special code for variable initialization. This is not yet supported by MTK