add state priority option for rotation matrices

Author: baggepinnen

src/Multibody.jl

@@ -82,12 +82,15 @@ end
 
 Emulates the `@variables` macro but does never creates array variables. Also never introuces the variable names into the calling scope.
 """
-function at_variables_t(args...; default = nothing)
+function at_variables_t(args...; default = nothing, state_priority = nothing)
     xs = Symbolics.variables(args...; T = Symbolics.FnType)
     xs = map(x -> x(t), xs)
     if default !== nothing
         xs = Symbolics.setdefaultval.(xs, default)
     end
+    if state_priority !== nothing
+        xs = Symbolics.setmetadata.(xs, ModelingToolkit.VariableStatePriority, state_priority)
+    end
     xs
 end
 

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)
+    R = NumRotationMatrix(; name, varw, state_priority=0)
 
     ODESystem(Equation[], t, [r_0; f; tau], []; name,
               metadata = Dict(:orientation => R, :frame => true))

src/orientation.jl

@@ -37,8 +37,8 @@ The primary difference between `NumRotationMatrix` and `RotationMatrix` is that
 
 - `varw`: If true, `w` is a variable, otherwise it is derived from the derivative of `R` as `w = get_w(R)`.
 """
-function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name, varw = false)
-    R = at_variables_t(:R, 1:3, 1:3, default = R) #[description="Orientation rotation matrix ∈ SO(3)"]
+function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name, varw = false, state_priority=nothing)
+    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)
     # R = ModelingToolkit.renamespace.(name, R) .|> Num