non-unit quaternion in Body

src/components.jl

@@ -265,17 +265,9 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  eqs = if isroot # isRoot
 
  if useQuaternions
- @named frame_a = Frame(varw = true)
- Ra = ori(frame_a, true)
- @named Q = NumQuaternion(varw=true) 
- ar = from_Q(Q, angularVelocity2(Q, D.(Q.Q)))
- Equation[
- 0 ~ orientation_constraint(Q)
- Ra ~ ar
- Ra.w .~ ar.w
- Q.w .~ ar.w # These should not be needed
- collect(w_a .~ Ra.w) # These should not be needed
- ]
+ @named frame_a = Frame(varw = false)
+ Ra = ori(frame_a, false)
+ nonunit_quaternion_equations(Ra, w_a);
  else
  @named frame_a = Frame(varw = true)
  @variables phi(t)[1:3]=phi0 [state_priority = 10, description = "Euler angles"]

src/orientation.jl

@@ -260,13 +260,16 @@ orientation_constraint(q::Quaternion) = orientation_constraint(q.Q)
 # 2 * Q * q̇
 # 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(Q, w)
- R = 2*[(Q[1]*Q[1] + Q[4]*Q[4])-1 (Q[1]*Q[2] + Q[3]*Q[4]) (Q[1]*Q[3] - Q[2]*Q[4]);
- (Q[2]*Q[1] - Q[3]*Q[4]) (Q[2]*Q[2] + Q[4]*Q[4])-1 (Q[2]*Q[3] + Q[1]*Q[4]);
- (Q[3]*Q[1] + Q[2]*Q[4]) (Q[3]*Q[2] - Q[1]*Q[4]) (Q[3]*Q[3] + Q[4]*Q[4])-1]
+ # TODO: shift Q elements 1,4
+ Q2 = [Q[4], Q[1], Q[2], Q[3]]
+ q = Rotations.QuatRotation(Q2)
+ R = RotMatrix(q)
  RotationMatrix(R, w)
 end
 
+
 function angularVelocity1(Q, der_Q)
  2*([Q[4] -Q[3] Q[2] -Q[1]; Q[3] Q[4] -Q[1] -Q[2]; -Q[2] Q[1] Q[4] -Q[3]]*der_Q)
 end
@@ -418,4 +421,30 @@ end
 
 function resolveDyade2(R, D1)
  R*D1*R'
+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 n(t)=1 c(t)=0
+ @parameters k = 0.1
+ Q̂ = collect(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]
+ P = [
+ 0 0 0 1
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ ]
+ Ω = P'Ω*P
+ [
+ n ~ Q̂'Q̂
+ # n ~ _norm(Q̂)^2
+ c ~ k * (1 - n)
+ D.(Q̂) .~ (Ω * Q̂) ./ 2 + c * Q̂
+ Q .~ Q̂ ./ sqrt(n)
+ R ~ from_Q(Q, w)
+ # R.w ~ w
+ ]
 end

test/runtests.jl

@@ -991,10 +991,16 @@ tt = 0:0.1:10
 # ==============================================================================
 ## Simple pendulum with quaternions=============================================================
 # ==============================================================================
-using LinearAlgebra, ModelingToolkit
+using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler
+t = Multibody.t
 # @testset "Simple pendulum" begin
-@named joint = Multibody.FreeMotion(isroot = true, enforceState=true, useQuaternions=true)
-@named body = Body(; m = 1, r_cm = [0.5, 0, 0], isroot=false, useQuaternions=false)
+@named joint = Multibody.FreeMotion(isroot = true, enforceState=false, useQuaternions=true)
+@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, useQuaternions=true, w_a=[1,0.5,0.2])
+
+# @named joint = Multibody.FreeMotion(isroot = true, enforceState=true, useQuaternions=true)
+# @named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=false, w_a=[1,1,1])
+
+world = Multibody.world
 
 
 connections = [connect(world.frame_b, joint.frame_a)
@@ -1010,10 +1016,56 @@ ssys = structural_simplify(irsys)
 
 ##
 D = Differential(t)
-prob = ODEProblem(ssys, [collect(joint.w_rel_b .=> randn(3)); ], (0, 10))
+prob = ODEProblem(ssys, [collect(body.w_a) .=> [1,0,0];], (0, 2pi))
 
-using OrdinaryDiffEq
-sol = solve(prob, Rodas4())
+using OrdinaryDiffEq, Test
+sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 0 .* randn.())#, initializealg=ShampineCollocationInit(0.01))
+# sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 1e-12 .* randn.(), dtmin=1e-8, force_dtmin=true)
 @test SciMLBase.successful_retcode(sol)
-doplot() && plot(sol)
+doplot() && plot(sol, layout=13)
 # end
+
+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
+
+
+
+function get_R(frame, t)
+ reshape(sol(t, idxs=vec(ori(frame).R.mat)), 3, 3)
+end
+function get_r(frame, t)
+ sol(t, idxs=collect(frame.r_0))
+end
+
+import Multibody.Rotations.QuatRotation as Quat
+import Multibody.Rotations
+ti = 5
+for (i, ti) in enumerate(ts)
+ R = get_R(body.frame_a, ti)
+ @test norm(R'R - I) < 1e-10
+ @test Multibody.from_Q(Q[:, i], 0).R ≈ R atol=1e-3
+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, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3
+@test get_R(body.frame_a, 2pi) ≈ I atol=1e-3
+
+prob = ODEProblem(ssys, [collect(body.w_a) .=> [0,1,0];], (0, 2pi))
+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
+
+
+prob = ODEProblem(ssys, [collect(body.w_a) .=> [0,0,1];], (0, 2pi))
+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