diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md
index fe6c5595..ec160c23 100644
--- a/docs/src/examples/free_motion.md
+++ b/docs/src/examples/free_motion.md
@@ -49,12 +49,12 @@ 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.
+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`.
 
 ```@example FREE_MOTION
 @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)
+                     r_0 = [0.2, -0.5, 0.1], isroot = true, quat=true)
     bar2 = FixedTranslation(r = [0.8, 0, 0])
     spring1 = Multibody.Spring(c = 20, s_unstretched = 0)
     spring2 = Multibody.Spring(c = 20, s_unstretched = 0)
@@ -78,7 +78,9 @@ ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [
     collect(body.body.v_0 .=> 0);
     collect(body.body.w_a .=> 0);
-], (0, 3))
+    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]));
+], (0, 4))
 
 sol = solve(prob, Rodas5P())
 @assert SciMLBase.successful_retcode(sol)
diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md
index 9dcb7904..dbffe00b 100644
--- a/docs/src/examples/pendulum.md
+++ b/docs/src/examples/pendulum.md
@@ -155,7 +155,7 @@ defs = Dict(collect(multibody_spring.r_rel_0 .=> [0, 1, 0])...,
             collect(root_body.v_0 .=> [0, 0, 0])...,
 )
 
-prob = ODEProblem(ssys, defs, (0, 15))
+prob = ODEProblem(ssys, defs, (0, 10))
 
 sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.())
 plot(sol, idxs = multibody_spring.r_rel_0[2], title="Mass-spring system without joint")
@@ -169,7 +169,7 @@ push!(connections, connect(multibody_spring.spring2d.flange_b, damper.flange_b))
 
 @named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body, damper])
 ssys = structural_simplify(IRSystem(model))
-prob = ODEProblem(ssys, defs, (0, 15))
+prob = ODEProblem(ssys, defs, (0, 10))
 
 sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.())
 plot(sol, idxs = multibody_spring.r_rel_0[2], title="Mass-spring-damper without joint")
diff --git a/docs/src/examples/three_springs.md b/docs/src/examples/three_springs.md
index 98fc7f41..7b2d6f50 100644
--- a/docs/src/examples/three_springs.md
+++ b/docs/src/examples/three_springs.md
@@ -20,14 +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.2, 0], isroot = false)
+                 r_cm = [0, -0.2, 0], isroot=true, useQuaternions=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,
                                r_rel_0 = [-0.2, -0.2, 0.2])
-    spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1,
-                               fixed_rotation_at_frame_a = true, fixed_rotation_at_frame_b = 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, fixedRotationAtFrame_b=true)
 end
 eqs = [connect(world.frame_b, bar1.frame_a)
        connect(world.frame_b, bar2.frame_a)
@@ -39,15 +38,13 @@ eqs = [connect(world.frame_b, bar1.frame_a)
 
 @named model = ODESystem(eqs, t, systems = [world; systems])
 ssys = structural_simplify(IRSystem(model))
-prob = ODEProblem(ssys, [
-    D.(spring3.lineforce.r_rel_0) .=> 0;
-    collect(body1.v_0) .=> 0;
-], (0, 10))
+prob = ODEProblem(ssys, [], (0, 10))
 
-sol = solve(prob, Rodas4(), u0=prob.u0 .+ 1e-1*randn(length(prob.u0)))
+sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-9*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])
 ```
 
 ## 3D animation
diff --git a/src/components.jl b/src/components.jl
index f35f3726..efb7b0f0 100644
--- a/src/components.jl
+++ b/src/components.jl
@@ -222,6 +222,8 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
               phid0 = zeros(3),
               r_0 = 0,
               radius = 0.05,
+              v_0 = 0,
+              w_a = 0,
               air_resistance = 0.0,
               color = [1,0,0,1],
               quat=false,)
@@ -229,7 +231,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
         state_priority = 2,
         description = "Position vector from origin of world frame to origin of frame_a",
     ]
-    @variables v_0(t)[1:3] [guess = 0, 
+    @variables v_0(t)[1:3]=v_0 [guess = 0, 
         state_priority = 2,
         description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
     ]
@@ -238,7 +240,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
         description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
     ]
     @variables g_0(t)[1:3] [guess = 0, description = "gravity acceleration"]
-    @variables w_a(t)[1:3] [guess = 0, 
+    @variables w_a(t)[1:3]=w_a [guess = 0, 
         state_priority = 2,
         description = "Absolute angular velocity of frame_a resolved in frame_a",
     ]
@@ -273,39 +275,25 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
     eqs = if isroot # isRoot
         
         if quat
-            @named frame_a = Frame(varw = true)
-            Ra = ori(frame_a, true)
-            # @variables q(t)[1:4] = [0.0,0,0,1.0]
-            # @variables qw(t)[1:3] = [0.0,0,0]
-            # q = collect(q)
-            # qw = collect(qw)
-            # Q = Quaternion(q, qw)
-            @named Q = NumQuaternion(varw=true) 
-            ar = from_Q(Q, angular_velocity2(Q, D.(Q.Q)))
-            Equation[
-                0 ~ orientation_constraint(Q)
-                Ra ~ ar
-                Ra.w .~ ar.w
-                Q.w .~ ar.w
-                collect(w_a .~ Ra.w)
-            ]
+            @named frame_a = Frame(varw = false)
+            Ra = ori(frame_a, false)
+            nonunit_quaternion_equations(Ra, w_a);
         else
             @named frame_a = Frame(varw = true)
+            Ra = ori(frame_a, true)
             @variables phi(t)[1:3]=phi0 [state_priority = 10, description = "Euler angles"]
             @variables phid(t)[1:3]=phid0 [state_priority = 10]
             @variables phidd(t)[1:3]=zeros(3) [state_priority = 10]
             phi, phid, phidd = collect.((phi, phid, phidd))
             ar = axes_rotations([1, 2, 3], phi, phid)
-
-            Ra = ori(frame_a, true)
-
             Equation[
-                    # 0 .~ orientation_constraint(Ra); 
                     phid .~ D.(phi)
                     phidd .~ D.(phid)
                     Ra ~ ar
-                    Ra.w .~ ar.w
-                    collect(w_a .~ Ra.w)]
+                    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
+                    ]
         end
     else
         @named frame_a = Frame()
@@ -318,7 +306,6 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
     end
 
     eqs = [eqs;
-           # collect(w_a .~ get_w(Ra));
            collect(r_0 .~ frame_a.r_0)
            collect(g_0 .~ gravity_acceleration(frame_a.r_0 .+ resolve1(Ra, r_cm)))
            collect(v_0 .~ D.(r_0))
diff --git a/src/forces.jl b/src/forces.jl
index c3f2925c..3c2906cb 100644
--- a/src/forces.jl
+++ b/src/forces.jl
@@ -140,9 +140,9 @@ function Force(; name, resolve_frame = :frame_b)
 end
 
 function LineForceBase(; name, length = 0, s_small = 1e-10, fixed_rotation_at_frame_a = false,
-                       fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0)
-    @named frame_a = Frame()
-    @named frame_b = Frame()
+    fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0)
+    @named frame_a = Frame(varw = fixed_rotation_at_frame_a)
+    @named frame_b = Frame(varw = fixed_rotation_at_frame_b)
 
     @variables length(t) [
         description = "Distance between the origin of frame_a and the origin of frame_b",
diff --git a/src/joints.jl b/src/joints.jl
index f1a03973..670e39e1 100644
--- a/src/joints.jl
+++ b/src/joints.jl
@@ -177,7 +177,9 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
                    d = 0,
                    phi_dd = 0,
                    color = [1, 1, 0, 1],
-                   radius = 0.1)
+                   radius = 0.1,
+                   quat = false,
+                   )
     @named begin
         ptf = PartialTwoFrames()
         R_rel = NumRotationMatrix()
@@ -209,19 +211,24 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
     end
 
     if state
-        @variables begin
-            (phi(t)[1:3] = phi),
-            [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"]
-            (phi_d(t)[1:3] = phi_d),
-            [state_priority = 10, description = "3 angle derivatives"]
-            (phi_dd(t)[1:3] = phi_dd),
-            [state_priority = 10, description = "3 angle second derivatives"]
+        if quat
+            append!(eqs, nonunit_quaternion_equations(R_rel, w_rel))
+            # append!(eqs, collect(w_rel) .~ angularVelocity2(R_rel))
+        else
+            @variables begin
+                (phi(t)[1:3] = phi),
+                [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"]
+                (phi_d(t)[1:3] = phi_d),
+                [state_priority = 10, description = "3 angle derivatives"]
+                (phi_dd(t)[1:3] = phi_dd),
+                [state_priority = 10, description = "3 angle second derivatives"]
+            end
+            append!(eqs,
+                    [R_rel ~ axes_rotations(sequence, phi, phi_d)
+                    collect(w_rel) .~ angular_velocity2(R_rel)
+                    collect(phi_d .~ D.(phi))
+                    collect(phi_dd .~ D.(phi_d))])
         end
-        append!(eqs,
-                [R_rel ~ axes_rotations(sequence, phi, phi_d)
-                 collect(w_rel) .~ angular_velocity2(R_rel)
-                 collect(phi_d .~ D.(phi))
-                 collect(phi_dd .~ D.(phi_d))])
         if isroot
             append!(eqs,
                     [connect_orientation(ori(frame_b), absolute_rotation(frame_a, R_rel); iscut)
@@ -682,7 +689,9 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
 - `a_rel_a`
 """
 @component function FreeMotion(; name, state = true, sequence = [1, 2, 3], isroot = true,
+                    quat = false,
                     w_rel_a_fixed = false, z_rel_a_fixed = false, phi = 0,
+                    iscut = false,
                     phi_d = 0,
                     phi_dd = 0,
                     w_rel_b = 0,
@@ -695,13 +704,13 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
     end
     @variables begin
         (phi(t)[1:3] = phi),
-        [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"]
-        (phi_d(t)[1:3] = phi_d), [state_priority = 10, description = "Derivatives of phi"]
+        [state_priority = 4, description = "3 angles to rotate frame_a into frame_b"]
+        (phi_d(t)[1:3] = phi_d), [state_priority = 4, description = "Derivatives of phi"]
         (phi_dd(t)[1:3] = phi_dd),
-        [state_priority = 10, description = "Second derivatives of phi"]
+        [state_priority = 4, description = "Second derivatives of phi"]
         (w_rel_b(t)[1:3] = w_rel_b),
         [
-            state_priority = 10,
+            state_priority = quat ? 4.0 : 1.0,
             description = "relative angular velocity of frame_b with respect to frame_a, resolved in frame_b",
         ]
         (r_rel_a(t)[1:3] = r_rel_a),
@@ -733,18 +742,24 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
     if state
         if isroot
             append!(eqs,
-                    ori(frame_b) ~ absolute_rotation(frame_a, R_rel))
+                    connect_orientation(ori(frame_b), absolute_rotation(frame_a, R_rel); iscut))
         else
             append!(eqs,
                     [R_rel_inv ~ inverse_rotation(R_rel)
-                     ori(frame_a) ~ absolute_rotation(frame_b, R_rel_inv)])
+                     connect_orientation(ori(frame_a), absolute_rotation(frame_b, R_rel_inv); iscut)])
         end
 
-        append!(eqs,
-                [phi_d .~ D.(phi)
-                 phi_dd .~ D.(phi_d)
-                 R_rel ~ axes_rotations(sequence, phi, phi_d)
-                 w_rel_b .~ angular_velocity2(R_rel)])
+        if quat
+            # @named Q = NumQuaternion() 
+            append!(eqs, nonunit_quaternion_equations(R_rel, w_rel_b))
+
+        else
+            append!(eqs,
+                    [phi_d .~ D.(phi)
+                    phi_dd .~ D.(phi_d)
+                    R_rel ~ axes_rotations(sequence, phi, phi_d)
+                    w_rel_b .~ angular_velocity2(R_rel)])
+        end
 
     else
         # Free motion joint does not have state
diff --git a/src/orientation.jl b/src/orientation.jl
index 43cf2246..74878428 100644
--- a/src/orientation.jl
+++ b/src/orientation.jl
@@ -273,13 +273,17 @@ 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]
+    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]]
+
 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
@@ -475,4 +479,29 @@ function get_frame(sol, frame, t)
     R = get_rot(sol, frame, t)
     tr = get_trans(sol, frame, t)
     [R tr; 0 0 0 1]
+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 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
\ No newline at end of file
diff --git a/test/runtests.jl b/test/runtests.jl
index 951de54e..3aea30c9 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -58,9 +58,8 @@ t = Multibody.t
 D = Differential(t)
 @testset "spring - harmonic oscillator" begin
 
-    @named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], phi0 = [0, 1, 0], quat=false) # This time the body isroot since there is no joint containing state
-    @named spring = Multibody.Spring(c = 1, fixed_rotation_at_frame_a = false,
-                                    fixed_rotation_at_frame_b = false)
+    @named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], quat=false) # This time the body isroot since there is no joint containing state
+    @named spring = Multibody.Spring(c = 1)
 
     connections = [connect(world.frame_b, spring.frame_a)
                 connect(spring.frame_b, body.frame_a)]
@@ -77,7 +76,10 @@ D = Differential(t)
     # @test all(isfinite, du)
 
     # @test_skip begin # Yingbo: instability
-    prob = ODEProblem(ssys, unknowns(ssys) .=> 0, (0, 10))
+    prob = ODEProblem(ssys, [
+        collect(body.w_a) .=> 0.01;
+        collect(body.v_0) .=> 0;
+    ], (0, 10))
     sol = solve(prob, Rodas5P(), u0 = prob.u0 .+ 1e-5 .* randn.())
     @test SciMLBase.successful_retcode(sol)
     @test sol(2pi, idxs = body.r_0[1])≈0 atol=1e-3
@@ -86,7 +88,7 @@ D = Differential(t)
     @test sol(pi, idxs = body.r_0[2]) < -2
 
     doplot() &&
-        plot(sol, idxs = [collect(body.r_0); collect(body.v_0); collect(body.phi)], layout = 9)
+        plot(sol, idxs = [collect(body.r_0); collect(body.v_0)], layout = 6)
 end
 
 # ==============================================================================
@@ -314,7 +316,7 @@ end
 @named body = Body(; m = 1, isroot = false, r_cm = [0, 0, 0])
 @named damper = Translational.Damper(d=0.5)
 @named spring = Translational.Spring(c=1)
-@named joint = Prismatic(n = [0, 1, 0], isroot = true, axisflange = true)
+@named joint = Prismatic(n = [0, 1, 0], axisflange = true)
 
 connections = [connect(world.frame_b, joint.frame_a)
                connect(damper.flange_b, spring.flange_b, joint.axis)
@@ -347,7 +349,7 @@ systems = @named begin
     bar1 = FixedTranslation(r = [0.3, 0, 0])
     bar2 = FixedTranslation(r = [0.6, 0, 0])
     bar3 = FixedTranslation(r = [0.9, 0, 0])
-    p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true, isroot = true)
+    p2 = Prismatic(n = [0, -1, 0], s0 = 0.1, axisflange = true)
     spring2 = Multibody.Spring(c = 30, s_unstretched = 0.1)
     spring1 = Multibody.Spring(c = 30, s_unstretched = 0.1)
     damper1 = Multibody.Damper(d = 2)
@@ -463,7 +465,6 @@ sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 1e-1 .* rand.())
 
 doplot() && plot(sol, idxs = [body1.r_0...]) |> display
 # end
-# TODO: add tutorial explaining what interesting things this demos illustrates
 # fixed_rotation_at_frame_a and b = true required
 end
 # ==============================================================================
@@ -483,7 +484,7 @@ world = Multibody.world
 
 @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], r_cm = [0.2, 0, 0], isroot = true)
+                     r_0 = [0.2, -0.5, 0.1], r_cm = [0.2, 0, 0], isroot = true, quat=true)
     bar2 = FixedTranslation(r = [0.8, 0, 0])
     spring1 = Multibody.Spring(c = 20, s_unstretched = 0)
     spring2 = Multibody.Spring(c = 20, s_unstretched = 0)
@@ -506,20 +507,29 @@ eqs = [connect(bar2.frame_a, world.frame_b)
 ssys = structural_simplify(IRSystem(model))#, alias_eliminate = true)
 # ssys = structural_simplify(model, allow_parameters = false)
 prob = ODEProblem(ssys,
-                  [collect(body.body.w_a .=> 0);
+                  [world.g => 9.80665;
+                    collect(body.body.w_a .=> 0);
                   collect(body.body.v_0 .=> 0);
-                   collect(D.(body.body.phi)) .=> 1;
-                #    collect((body.body.r_0)) .=> collect((body.r_0));
-                   collect(D.(D.(body.body.phi))) .=> 1], (0, 10))
+                #   collect(body.body.phi .=> deg2rad(10));
+                    collect(body.body.Q) .=> Multibody.to_mb(Rotations.QuatRotation(Rotations.RotXYZ(deg2rad.((10,10,10))...)));
+                    collect(body.body.Q̂) .=> Multibody.to_mb(Rotations.QuatRotation(Rotations.RotXYZ(deg2rad.((10,10,10))...)));
+                  ], (0, 10))
 
 # @test_skip begin # The modelica example uses angles_fixed = true, which causes the body component to run special code for variable initialization. This is not yet supported by MTK
-# Without proper initialization, the example fails most of the time. Random perturbation of u0 can make it work sometimes.
-sol = solve(prob, Rodas4())
+sol = solve(prob, Rodas5P(), abstol=1e-6, reltol=1e-6)
 @test SciMLBase.successful_retcode(sol)
 
-@info "Initialization broken, initial value for body.r_0 not respected, add tests when MTK has a working initialization"
-doplot() && plot(sol, idxs = [body.r_0...]) |> display
-# end
+@test sol(10, idxs=[
+    body.r_0;
+]) ≈ [
+    0.196097
+    -0.46243
+    0.080562
+] atol=1e-1
+
+@test_broken get_rot(sol, body.frame_b, 10)[1,2] ≈ 0.104409 atol=0.01
+
+doplot() && plot(sol, idxs = [body.r_0...; body.body.w_a; body.body.v_0], layout=(3,3), size=(1000, 1000)) |> display
 
 end
 # ==============================================================================
@@ -812,7 +822,7 @@ end
 # ==============================================================================
 ## Harmonic oscillator with Body as root and quaternions as state variables
 # ==============================================================================
-
+using LinearAlgebra
 @testset "Harmonic oscillator with Body as root and quaternions as state variables" begin
 
 @named body = Body(; m = 1, isroot = true, r_cm = [0.0, 0, 0], phi0 = [0, 0.9, 0], quat=true) # This time the body isroot since there is no joint containing state
@@ -1008,3 +1018,201 @@ sol = solve(prob, Rodas4())
 tt = 0:0.1:10
 @test Matrix(sol(tt, idxs = [collect(body.r_0[2:3]);])) ≈ Matrix(sol(tt, idxs = [collect(body2.r_0[2:3]);]))
 
+
+
+
+# ==============================================================================
+## Simple motion with quaternions=============================================================
+# ==============================================================================
+using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler
+t = Multibody.t
+@named joint = Multibody.FreeMotion(isroot = true, state=false, quat=true)
+@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, quat=true, w_a=[1,0.5,0.2])
+
+# @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=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)
+               connect(joint.frame_b, body.frame_a)]
+
+
+@named model = ODESystem(connections, t,
+                         systems = [world, joint, body])
+irsys = IRSystem(model)
+ssys = structural_simplify(irsys)
+
+
+
+##
+D = Differential(t)
+prob = ODEProblem(ssys, [collect(body.w_a) .=> [1,0,0];], (0, 2pi))
+
+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, 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, pi/2) ≈ [1 0 0; 0 0 -1; 0 1 0] atol=1e-3
+@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
+
+
+
+## states in joint instead of body
+
+using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler
+t = Multibody.t
+world = Multibody.world
+
+@named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true)
+@named body = Body(; m = 1, r_cm = [0.0, 0, 0])
+
+connections = [connect(world.frame_b, joint.frame_a)
+               connect(joint.frame_b, body.frame_a)]
+
+@named model = ODESystem(connections, t,
+                         systems = [world, joint, body])
+irsys = IRSystem(model)
+ssys = structural_simplify(irsys)
+
+##
+D = Differential(t)
+# q0 = randn(4); q0 ./= norm(q0)
+q0 = [0,0,0,1]
+prob = ODEProblem(ssys, [
+    collect(body.w_a) .=> [1,1,1];
+    collect(joint.Q) .=> q0;
+    collect(joint.Q̂) .=> q0;
+    ], (0, 2pi))
+
+using OrdinaryDiffEq, Test
+sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 1e-6 .* randn.())
+@test SciMLBase.successful_retcode(sol)
+# doplot() && plot(sol, layout=21)
+
+
+ts = 0:0.1:2pi
+Q = Matrix(sol(ts, idxs = [joint.Q...]))
+Qh = Matrix(sol(ts, idxs = [joint.Q̂...]))
+n = Matrix(sol(ts, idxs = [joint.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
+
+@test get_R(joint.frame_b, 0pi) ≈ I
+@test_broken get_R(joint.frame_b, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3
+@test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3
+
+Matrix(sol(ts, idxs = [joint.w_rel_b...]))
+
+
+
+
+
+## Spherical joint pendulum with quaternions
+
+using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler
+t = Multibody.t
+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, air_resistance=0.0)
+
+# @named joint = Multibody.Spherical(isroot=true, state=true, quat=true)
+# @named body = Body(; m = 1, r_cm = [1.0, 0, 0], isroot=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)
+@test_skip begin # https://github.com/JuliaComputing/JuliaSimCompiler.jl/issues/298
+    ssys = structural_simplify(irsys)
+
+
+    D = Differential(t)
+    q0 = randn(4); q0 ./= norm(q0)
+    # q0 = [0,0,0,1]
+    prob = ODEProblem(ssys, [
+        # collect(body.w_a) .=> [1,0,0];
+        # collect(body.Q) .=> q0;
+        # collect(body.Q̂) .=> q0;
+        ], (0, 30))
+
+    using OrdinaryDiffEq, Test
+    sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 0 .* randn.())
+    @test SciMLBase.successful_retcode(sol)
+    # doplot() && plot(sol, layout=21)
+
+
+    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
+
+    Matrix(sol(ts, idxs = [body.w_a...]))
+
+    @test get_R(joint.frame_b, 0pi) ≈ I
+    @test get_R(joint.frame_b, sqrt(9.81/1)) ≈ diagm([1, -1, -1]) atol=1e-3
+    @test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3
+
+
+    Matrix(sol(ts, idxs = [joint.w_rel_b...]))
+
+    # render(model, sol)
+end
\ No newline at end of file