diff --git a/examples/fourbar.jl b/examples/fourbar.jl
new file mode 100644
index 00000000..0c05cf2f
--- /dev/null
+++ b/examples/fourbar.jl
@@ -0,0 +1,190 @@
+using Multibody, OrdinaryDiffEq, JuliaSimCompiler
+using ModelingToolkit
+using Multibody: connect_loop
+using Test
+t = Multibody.t
+
+world = Multibody.world
+
+
+W(args...; kwargs...) = Multibody.world
+
+# @mtkmodel FourBar begin
+#     @variables begin
+#         j1_phi(t), [description = "Angle of revolute joint j1", output = true]
+#         j2_s(t), [description = "Distance of prismatic joint j2", output = true]
+#         j1_w(t), [description = "Axis speed of revolute joint j1", output = true]
+#         j2_v(t), [description = "Axis velocity of prismatic joint j2", output = true]
+#     end
+#     @components begin
+#         world = W()
+#         j1 = Revolute(
+#             n=[1,0,0],
+#             # stateSelect=StateSelect.always,
+#             w0=5.235987755982989,
+#             # fixed=true
+#             isroot = false,
+#             )
+#         j2 = Prismatic(n=[1,0,0], s0 = -0.2, isroot=false)
+#         b1 = BodyShape(r=[0,0.5,0.1], isroot=false)
+#         b2 = BodyShape(r=[0,0.2,0], isroot=false)
+#         b3 = BodyShape(r=[-1,0.3,0.1])
+#         rev = Revolute(n=[0,1,0], isroot=false)
+#         rev1 = Revolute(isroot=true)
+#         j3 = Revolute(n=[1,0,0])
+#         j4 = Revolute(n=[0,1,0])
+#         j5 = Revolute(n=[0,0,1])
+#         b0 = FixedTranslation(r=[1.2,0,0])
+#     end
+
+#     @equations begin
+#         connect(j2.frame_b, b2.frame_a)
+#         connect(j1.frame_b, b1.frame_a)
+#         # connect(rev.frame_a, b2.frame_b)
+#         connect_loop(rev.frame_a, b2.frame_b)
+#         connect(rev.frame_b, rev1.frame_a)
+#         connect(rev1.frame_b, b3.frame_a) 
+#         connect(world.frame_b, j1.frame_a)
+#         connect(b1.frame_b, j3.frame_a)
+#         connect(j3.frame_b, j4.frame_a)
+#         connect(j4.frame_b, j5.frame_a)
+#         connect(j5.frame_b, b3.frame_b)
+#         connect(b0.frame_a, world.frame_b)
+#         connect(b0.frame_b, j2.frame_a)
+#         j1_phi ~ j1.phi
+#         j2_s ~ j2.s
+#         j1_w ~ j1.w
+#         j2_v ~ j2.v
+#     end
+# end
+
+# @named fourbar = FourBar()
+@named begin
+    j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989, isroot = false)
+    j2 = Prismatic(n = [1, 0, 0], s0 = -0.2, isroot=true)
+    b1 = BodyShape(r = [0, 0.5, 0.1])
+    b2 = BodyShape(r = [0, 0.2, 0])
+    b3 = BodyShape(r = [-1, 0.3, 0.1])
+    rev = Revolute(n = [0, 1, 0])
+    rev1 = Revolute()
+    j3 = Revolute(n = [1, 0, 0])
+    j4 = Revolute(n = [0, 1, 0])
+    j5 = Revolute(n = [0, 0, 1])
+    b0 = FixedTranslation(r = [1.2, 0, 0])
+end
+
+connections = [connect(j2.frame_b, b2.frame_a)
+               connect(j1.frame_b, b1.frame_a)
+               Multibody.connect_loop(rev.frame_a, b2.frame_b)
+            #    connect(rev.frame_a, b2.frame_b)
+               connect(rev.frame_b, rev1.frame_a)
+               connect(rev1.frame_b, b3.frame_a)
+               connect(world.frame_b, j1.frame_a)
+               connect(b1.frame_b, j3.frame_a)
+               connect(j3.frame_b, j4.frame_a)
+               connect(j4.frame_b, j5.frame_a)
+               connect(j5.frame_b, b3.frame_b)
+               connect(b0.frame_a, world.frame_b)
+               connect(b0.frame_b, j2.frame_a)]
+@named fourbar = ODESystem(connections, t,
+                         systems = [
+                             world,
+                             j1,
+                             j2,
+                             b1,
+                             b2,
+                             b3,
+                             rev,
+                             rev1,
+                             j3,
+                             j4,
+                             j5,
+                             b0,
+                         ])
+
+# m = structural_simplify(fourbar)
+m = structural_simplify(IRSystem(fourbar))
+
+prob = ODEProblem(m, [], (0.0, 5.0))
+
+sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 0.01 .* randn.())
+
+
+
+# ==============================================================================
+## Trivial 4 bar
+# ==============================================================================
+
+
+# First test the structure without the loop closed, this makes a quadruple pendulum
+
+systems = @named begin
+    j1 = Revolute(isroot = true)
+    j2 = Revolute(isroot = true)
+    j3 = Revolute(isroot = true)
+    j4 = Revolute(isroot = true)
+    b1 = BodyShape(r = [1.0, 0, 0])
+    b2 = BodyShape(r = [1.0, 0, 0])
+    b3 = BodyShape(r = [1.0, 0, 0])
+    b4 = BodyShape(r = [1.0, 0, 0])
+end
+
+connections = [
+    connect(world.frame_b, j1.frame_a)
+    connect(j1.frame_b, b1.frame_a)
+    connect(b1.frame_b, j2.frame_a)
+    connect(j2.frame_b, b2.frame_a)
+    connect(b2.frame_b, j3.frame_a)
+    connect(j3.frame_b, b3.frame_a)
+    connect(b3.frame_b, j4.frame_a)
+    connect(j4.frame_b, b4.frame_a)
+]
+@named fourbar = ODESystem(connections, t, systems = [world; systems])
+
+# m = structural_simplify(fourbar)
+m = structural_simplify(IRSystem(fourbar))
+@test length(states(m)) == 8
+
+
+## Now close the loop
+# When we do, we can only select one joint as root. The loop closure must not use a regular connect statement, instead, we use connect_loop
+# For unknown reason, we must also change the connection to the world to use connect_loop for the system to balance
+systems = @named begin
+    j1 = Revolute(isroot = true)
+    j2 = Revolute(isroot = false)
+    j3 = Revolute(isroot = false)
+    j4 = Revolute(isroot = false)
+    b1 = BodyShape(r = [1.0, 0, 0])
+    b2 = BodyShape(r = [1.0, 0, 0])
+    b3 = BodyShape(r = [1.0, 0, 0])
+    b4 = BodyShape(r = [1.0, 0, 0])
+end
+
+connections = [
+    # connect(world.frame_b, j1.frame_a)
+    connect(j1.frame_b, b1.frame_a)
+    connect(b1.frame_b, j2.frame_a)
+    connect(j2.frame_b, b2.frame_a)
+    connect(b2.frame_b, j3.frame_a)
+    connect(j3.frame_b, b3.frame_a)
+    connect(b3.frame_b, j4.frame_a)
+    connect(j4.frame_b, b4.frame_a)
+    # connect(b4.frame_b, j1.frame_a)
+    Multibody.connect_loop(b4.frame_b, j1.frame_a)
+    Multibody.connect_loop(world.frame_b, j1.frame_a)
+]
+@named fourbar = ODESystem(connections, t, systems = [world; systems])
+
+m = structural_simplify(IRSystem(fourbar))
+
+@test_broken length(states(m)) == 2
+
+prob = ODEProblem(m, [], (0.0, 5.0))
+
+# Try the generated dynamics
+du = zero(prob.u0)
+prob.f.f(du, prob.u0, prob.p, 0)
+
+
+sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 0.01 .* randn.())
+isinteractive() && plot(sol, vars = [j1.phi, j2.phi, j3.phi, j4.phi])
\ No newline at end of file
diff --git a/src/orientation.jl b/src/orientation.jl
index 22ccce9d..0045472e 100644
--- a/src/orientation.jl
+++ b/src/orientation.jl
@@ -196,15 +196,23 @@ end
 
 orientation_constraint(R1, R2) = orientation_constraint(R1'R2)
 
-function residue(O1, O2)
+function residue(R1, R2)
     # https://github.com/modelica/ModelicaStandardLibrary/blob/master/Modelica/Mechanics/MultiBody/Frames/Orientation.mo
-    R1 = O1.R
-    R2 = O2.R
+    R1 isa ODESystem && (R1 = ori(R1))
+    R2 isa ODESystem && (R2 = ori(R2))
+    R1 = R1.R.mat
+    R2 = R2.R.mat
     [atan(cross(R1[1, :], R1[2, :]) ⋅ R2[2, :], R1[1, :] ⋅ R2[1, :])
      atan(-cross(R1[1, :], R1[2, :]) ⋅ R2[1, :], R1[2, :] ⋅ R2[2, :])
      atan(R1[2, :] ⋅ R2[1, :], R1[3, :] ⋅ R2[3, :])]
 end
 
+function connect_loop(F1, F2)
+    F1.metadata[:loop_opening] = true
+    # connect(F1, F2)
+    residue(F1, F2) .~ 0
+end
+
 ## Quaternions
 orientation_constraint(q::AbstractVector) = q'q - 1