From 3a27c215d60d6c687657a4930d906642c3eeac4f Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 24 Jun 2024 16:25:19 +0200 Subject: [PATCH 1/4] turn of quaternion normalization --- src/orientation.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/orientation.jl b/src/orientation.jl index e6449bad..57c981b0 100644 --- a/src/orientation.jl +++ b/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 From 9073368f1f7d75b079ac7e546a8a21014a6cea3a Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 24 Jun 2024 17:06:03 +0200 Subject: [PATCH 2/4] eliminate Q variable --- src/orientation.jl | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/orientation.jl b/src/orientation.jl index 57c981b0..f043b01f 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -404,7 +404,7 @@ 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="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 n(t)=1 c(t)=0 @parameters k = 0.1 @@ -417,12 +417,11 @@ 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) [ 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) - Q .~ Q̂ ./ sqrt(n) - R ~ QR + # Q .~ Q̂ ./ sqrt(n) + R ~ from_Q(Q̂ ./ sqrt(n), w) ] end \ No newline at end of file From 29979951aec0719dd309836bbae6d88aa620b2cb Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 24 Jun 2024 17:56:20 +0200 Subject: [PATCH 3/4] eliminate `n` --- src/orientation.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/orientation.jl b/src/orientation.jl index f043b01f..08eec8eb 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -409,7 +409,7 @@ function nonunit_quaternion_equations(R, w) @variables n(t)=1 c(t)=0 @parameters k = 0.1 Q̂ = collect(Q̂) - Q = collect(Q) + # Q = collect(Q) # w is used in Ω, and Ω determines D(Q̂) # This corresponds to modelica's # frame_a.R = from_Q(Q, angularVelocity2(Q, der(Q))); @@ -418,10 +418,10 @@ function nonunit_quaternion_equations(R, w) Ω = [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))) [ - n ~ Q̂'Q̂ - c ~ k * (1 - n) + # n ~ Q̂'Q̂ + c ~ k * (1 - Q̂'Q̂) 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) # Q .~ Q̂ ./ sqrt(n) - R ~ from_Q(Q̂ ./ sqrt(n), w) + R ~ from_Q(Q̂ ./ sqrt(Q̂'Q̂), w) ] end \ No newline at end of file From add1bf9b2d92c91744266e34174ea423bf933de0 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 25 Jun 2024 15:43:13 +0200 Subject: [PATCH 4/4] bump state priority Quat --- src/orientation.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/orientation.jl b/src/orientation.jl index 08eec8eb..bbd4e8a7 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -405,7 +405,7 @@ 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=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized @variables n(t)=1 c(t)=0 @parameters k = 0.1 Q̂ = collect(Q̂)