clean up

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

@@ -404,24 +404,29 @@ 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], [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 = collect(Q)
+ Q̂d = collect(Q̂d)
+ 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)));
  # where angularVelocity2(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)
  # 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̂ ./ sqrt(Q̂'Q̂), w)
  [
- # n ~ Q̂'Q̂
+ 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(Q̂'Q̂), w)
+ 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 ~ 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