pass along quaternion variables

Author: baggepinnen

src/components.jl

@@ -411,7 +411,10 @@ This component has a single frame, `frame_a`. To represent bodies with more than
         if quat
             @named frame_a = Frame(varw=false)
             Ra = ori(frame_a, false)
-            qeeqs = nonunit_quaternion_equations(Ra, w_a)
+            qeeqs, qvars, qpars = nonunit_quaternion_equations(Ra, w_a)
+            vars = [vars; qvars]
+            pars = [pars; qpars]
+            qeeqs
         else
             @named frame_a = Frame(varw=false)
             Ra = ori(frame_a, false)

src/joints.jl

@@ -225,7 +225,11 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
 
     if state
         if quat
-            append!(eqs, nonunit_quaternion_equations(Rrel, w_rel))
+            qeeqs, qvars, qpars = nonunit_quaternion_equations(Rrel, w_rel)
+            vars = [vars; qvars]
+            pars = [pars; qpars]
+            eqs = [eqs; qeeqs]
+            # append!(eqs, nonunit_quaternion_equations(Rrel, w_rel))
             # append!(eqs, collect(w_rel) .~ angularVelocity2(Rrel))
         else
             @variables begin
@@ -542,7 +546,11 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
         end
 
         if quat
-            append!(eqs, nonunit_quaternion_equations(Rrel, w_rel_b))
+            qeeqs, qvars, qpars = nonunit_quaternion_equations(Rrel, w_rel_b)
+            vars = [vars; qvars]
+            pars = [pars; qpars]
+            eqs = [eqs; qeeqs]
+            # append!(eqs, nonunit_quaternion_equations(Rrel, w_rel_b))
         else
             morevars = @variables begin
                 (phi(t)[1:3] = phi), [state_priority = state_priority, description = "3 angles to rotate frame_a into frame_b"]

src/orientation.jl

@@ -436,15 +436,20 @@ function get_frame(sol, frame, t)
 end
 
 function nonunit_quaternion_equations(R, w)
-    @variables Q(t)[1:4], [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], [state_priority=1000]
-    # NOTE: 
-    @variables n(t) c(t)
-    @parameters k = 0.1
-    Q̂ = collect(Q̂)
-    Q̂d = collect(Q̂d)
-    Q = collect(Q)
+    vars = @variables begin
+        Q(t)[1:4], [state_priority=-1, description="Unit quaternion with [w,i,j,k]"] # normalized
+        Q̂(t)[1:4]=[1,0,0,0], [state_priority=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized
+        Q̂d(t)[1:4], [state_priority=1000]
+        # NOTE: 
+        n(t)
+        c(t)
+    end
+    pars = @parameters begin
+        k = 0.1
+    end
+    # 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)));
@@ -454,7 +459,7 @@ function nonunit_quaternion_equations(R, w)
 
     # QR = from_Q(Q, angular_velocity2(Q, D.(Q)))
     QR = from_Q(Q̂ ./ sqrt(n), w)
-    [
+    eqs = [
         n ~ Q̂'Q̂
         c ~ k * (1 - n)
         D.(Q̂) .~ Q̂d
@@ -463,6 +468,7 @@ function nonunit_quaternion_equations(R, w)
         R ~ from_Q(Q, w)
         # R.w .~ angularVelocity2(Q̂, Q̂d)
     ]
+    eqs, vars, pars
 end
 
 # function angularVelocity2(Q, der_Q)