switch quaternion representation and add tests (#81)

* switch quaternion representation and add tests

* move testset

* docs improvement and clean up

docs/make.jl

@@ -10,7 +10,7 @@ makedocs(;
  authors = "JuliaHub Inc.",
  # strict = [:example_block, :setup_block, :eval_block],
  sitename = "Multibody.jl",
- warnonly = [:missing_docs, :cross_references, :example_block, :docs_block],
+ warnonly = [:missing_docs, :cross_references, :docs_block],
  pagesonly = true,
  format = Documenter.HTML(;
  prettyurls = get(ENV, "CI", nothing) == "true",

docs/src/examples/free_motion.md

@@ -49,9 +49,11 @@ nothing # hide
 ```
 
 
-If we instead model a body suspended in springs without the presence of any joint, we need to give the body state variables. We do this by saying `isroot = true` when we create the body, we also use quaternions to represent angles using `quat = true`.
+## Body suspended in springs
+If we instead model a body suspended in springs without the presence of any joint, we need to _give the body state variables_. We do this by saying `isroot = true` when we create the body, we also use quaternions to represent angular state using `quat = true`.
 
 ```@example FREE_MOTION
+using Multibody.Rotations: QuatRotation, RotXYZ
 @named begin
  body = BodyShape(m = 1, I_11 = 1, I_22 = 1, I_33 = 1, r = [0.4, 0, 0],
  r_0 = [0.2, -0.5, 0.1], isroot = true, quat=true)
@@ -78,14 +80,16 @@ ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [
  collect(body.body.v_0 .=> 0);
  collect(body.body.w_a .=> 0);
- collect(body.body.Q .=> Multibody.to_q([0.0940609, 0.0789265, 0.0940609, 0.987965]));
- collect(body.body.Q̂ .=> Multibody.to_q([0.0940609, 0.0789265, 0.0940609, 0.987965]));
+ collect(body.body.Q) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+ collect(body.body.Q̂) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
 ], (0, 4))
 
 sol = solve(prob, Rodas5P())
 @assert SciMLBase.successful_retcode(sol)
 
 plot(sol, idxs = [body.r_0...])
+# plot(sol, idxs = [body.body.Q...])
+
 ```
 
 ```@example FREE_MOTION

docs/src/examples/space.md

@@ -35,6 +35,7 @@ W(;kwargs...) = Multibody.world
  I_33=0.1,
  r_0=[0,0.6,0],
  isroot=true,
+ # quat=true, # Activate to use quaternions as state instead of Euler angles
  v_0=[1,0,0])
  body2 = Body(
  m=1,
@@ -43,6 +44,7 @@ W(;kwargs...) = Multibody.world
  I_33=0.1,
  r_0=[0.6,0.6,0],
  isroot=true,
+ # quat=true, # Activate to use quaternions as state instead of Euler angles
  v_0=[0.6,0,0])
  end
 end

docs/src/examples/three_springs.md

@@ -20,13 +20,13 @@ world = Multibody.world
 
 systems = @named begin
  body1 = Body(m = 0.8, I_11 = 0.1, I_22 = 0.1, I_33 = 0.1, r_0 = [0.5, -0.3, 0],
- r_cm = [0, 0, 0], isroot=false, quat=false)
+ r_cm = [0, -0.2, 0], isroot=true, quat=true)
  bar1 = FixedTranslation(r = [0.3, 0, 0])
  bar2 = FixedTranslation(r = [0, 0, 0.3])
- spring1 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1,
+ spring1 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1, fixed_rotation_at_frame_b=true,
  r_rel_0 = [-0.2, -0.2, 0.2])
- spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1, fixed_rotation_at_frame_b=true, fixed_rotation_at_frame_a=true)
- spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1)
+ spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1)
+ spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1, fixed_rotation_at_frame_a=false)
 end
 eqs = [connect(world.frame_b, bar1.frame_a)
  connect(world.frame_b, bar2.frame_a)
@@ -40,11 +40,29 @@ eqs = [connect(world.frame_b, bar1.frame_a)
 ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [], (0, 10))
 
-sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-9*randn(length(prob.u0)))
+sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-12*randn(length(prob.u0)))
 @assert SciMLBase.successful_retcode(sol)
 
 Plots.plot(sol, idxs = [body1.r_0...])
-# Plots.plot(sol, idxs = ori(body1.frame_a).R[1])
+```
+
+```@setup
+# States selected by Dymola
+# Statically selected continuous time states
+# body1.frame_a.r_0[1]
+# body1.frame_a.r_0[2]
+# body1.frame_a.r_0[3]
+# body1.v_0[1]
+# body1.v_0[2]
+# body1.v_0[3]
+# body1.w_a[1]
+# body1.w_a[2]
+# body1.w_a[3]
+
+# Dynamically selected continuous time states
+# There is one set of dynamic state selection.
+# There are 3 states to be selected from:
+# body1.Q[]
 ```
 
 ## 3D animation

src/components.jl

@@ -230,6 +230,8 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  I_31 = 0,
  I_32 = 0,
  isroot = false,
+ state = false,
+ vel_from_R = false,
  phi0 = zeros(3),
  phid0 = zeros(3),
  r_0 = 0,
@@ -283,13 +285,18 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
 
  # DRa = D(Ra)
 
+ if state
+ # @warn "Make the body have state variables by using isroot=true rather than state=true"
+ isroot = true
+ end
+
  dvs = [r_0;v_0;a_0;g_0;w_a;z_a;]
  eqs = if isroot # isRoot
 
  if quat
  @named frame_a = Frame(varw = false)
  Ra = ori(frame_a, false)
- nonunit_quaternion_equations(Ra, w_a);
+ qeeqs = nonunit_quaternion_equations(Ra, w_a)
  else
  @named frame_a = Frame(varw = true)
  Ra = ori(frame_a, true)
@@ -303,8 +310,11 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  phidd .~ D.(phid)
  Ra ~ ar
  Ra.w .~ w_a
- # w_a .~ angular_velocity2(ori(frame_a, false)) # This is required for FreeBody tests to pass, but the other one required for harmonic osciallator without joint to pass. FreeBody passes with quat=true so we use that instead
- w_a .~ ar.w # This one for most systems
+ if vel_from_R
+ w_a .~ angular_velocity2(ori(frame_a, false)) # This is required for FreeBody and ThreeSprings tests to pass, but the other one required for harmonic osciallator without joint to pass. FreeBody passes with quat=true so we use that instead
+ else
+ w_a .~ ar.w # This one for most systems
+ end
  ]
  end
  else

src/joints.jl

@@ -724,8 +724,10 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
  (a_rel_a(t)[1:3] = a_rel_a), [description = "= der(v_rel_a)"]
  end
 
- @named R_rel = NumRotationMatrix()
- @named R_rel_inv = NumRotationMatrix()
+ @named R_rel_f = Frame()
+ @named R_rel_inv_f = Frame()
+ R_rel = ori(R_rel_f)
+ R_rel_inv = ori(R_rel_inv_f)
 
  eqs = [
  # Cut-forces and cut-torques are zero
@@ -750,7 +752,6 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
  end
 
  if quat
- # @named Q = NumQuaternion() 
  append!(eqs, nonunit_quaternion_equations(R_rel, w_rel_b))
 
  else
@@ -769,7 +770,11 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
  R, angular_velocity1(frame_a)))
  end
  end
- compose(ODESystem(eqs, t; name), frame_a, frame_b)
+ if state && !isroot
+ compose(ODESystem(eqs, t; name), frame_a, frame_b, R_rel_f, R_rel_inv_f)
+ else
+ compose(ODESystem(eqs, t; name), frame_a, frame_b, R_rel_f, )
+ end
 end
 
 """

src/orientation.jl

@@ -36,8 +36,11 @@ Create a new [`RotationMatrix`](@ref) struct with symbolic elements. `R,w` deter
 The primary difference between `NumRotationMatrix` and `RotationMatrix` is that the `NumRotationMatrix` constructor is used in the constructor of a [`Frame`](@ref) in order to introduce the frame variables, whereas `RorationMatrix` (the struct) only wraps existing variables.
 
 - `varw`: If true, `w` is a variable, otherwise it is derived from the derivative of `R` as `w = get_w(R)`.
+
+Never call this function directly from a component constructor, instead call `f = Frame(); R = ori(f)` and add `f` to the subsystems.
 """
-function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name, varw = false, state_priority=nothing)
+function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name=:R, varw = false, state_priority=nothing)
+ # The reason for not calling this directly is that all R vaiables have to have the same name since they are treated as connector variables (otherwise a connection error is thrown). A component with more than one rotation matrix will thus have two different R variables that overwrite each other
  R = at_variables_t(:R, 1:3, 1:3; default = R, state_priority) #[description="Orientation rotation matrix ∈ SO(3)"]
  # @variables w(t)[1:3]=w [description="angular velocity"]
  # R = collect(R)
@@ -251,9 +254,13 @@ end
 
 Base.getindex(Q::Quaternion, i) = Q.Q[i]
 
-function NumQuaternion(; Q = [0.0, 0, 0, 1.0], w = zeros(3), name, varw = false)
+"""
+Never call this function directly from a component constructor, instead call `f = Frame(); R = ori(f)` and add `f` to the subsystems.
+"""
+function NumQuaternion(; Q = [1.0, 0, 0, 0.0], w = zeros(3), name, varw = false)
+ # The reason for not calling this directly is that all R vaiables have to have the same name since they are treated as connector variables (otherwise a connection error is thrown). A component with more than one rotation matrix will thus have two different R variables that overwrite each other
  # Q = at_variables_t(:Q, 1:4, default = Q) #[description="Orientation rotation matrix ∈ SO(3)"]
- @variables Q(t)[1:4] = [0.0,0,0,1.0]
+ @variables Q(t)[1:4] = [1.0,0,0,0]
  if varw
  @variables w(t)[1:3]=w [description="angular velocity"]
  # w = at_variables_t(:w, 1:3, default = w)
@@ -274,23 +281,26 @@ orientation_constraint(q::Quaternion) = orientation_constraint(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)
- Q2 = to_q(Q) # Due to different conventions
+function from_Q(Q2, w)
+ # Q2 = to_q(Q) # Due to different conventions
  q = Rotations.QuatRotation(Q2)
  R = RotMatrix(q)
  RotationMatrix(R, w)
 end
 
-to_q(Q) = SA[Q[4], Q[1], Q[2], Q[3]]
-to_mb(Q) = SA[Q[2], Q[3], Q[4], Q[1]]
+to_q(Q::AbstractVector) = SA[Q[4], Q[1], Q[2], Q[3]]
+to_q(Q::Rotations.QuatRotation) = to_q(vec(Q))
+to_mb(Q::AbstractVector) = SA[Q[2], Q[3], Q[4], Q[1]]
+to_mb(Q::Rotations.QuatRotation) = to_mb(vec(Q))
+Base.vec(Q::Rotations.QuatRotation) = SA[Q.q.s, Q.q.v1, Q.q.v2, Q.q.v3]
 
-function angular_velocity1(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
+# function angular_velocity1(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
 
-function angular_velocity2(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
+# function angular_velocity2(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
 
 
 ## Euler
@@ -446,6 +456,8 @@ The rotation matrix returned, ``R_W^F``, is such that when a vector ``p_F`` expr
 p_W = R_W^F p_F
 ```
 
+The columns of ``R_W_F`` indicate are the basis vectors of the frame ``F`` expressed in the world coordinate frame.
+
 See also [`get_trans`](@ref), [`get_frame`](@ref), [Orientations and directions](@ref) (docs section).
 """
 function get_rot(sol, frame, t)
@@ -481,26 +493,25 @@ function get_frame(sol, frame, t)
 end
 
 function nonunit_quaternion_equations(R, w)
- @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 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
  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)));
+ # 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]
- P = [
- 0 0 0 1
- 1 0 0 0
- 0 1 0 0
- 0 0 1 0
- ]
- Ω = P'Ω*P
+ # QR = from_Q(Q, angular_velocity2(Q, D.(Q)))
+ QR = from_Q(Q, w)
  [
  n ~ Q̂'Q̂
- # n ~ _norm(Q̂)^2
  c ~ k * (1 - n)
- D.(Q̂) .~ (Ω * Q̂) ./ 2 + c * 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, w)
- # R.w ~ w
+ R ~ QR
  ]
 end