From c786e750781b7d050737b6c7cbf38bad920235ba Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Mon, 24 Apr 2023 14:47:32 +0200
Subject: [PATCH] wip kinematic loops

---
 src/orientation.jl | 14 +++++++++---
 test/runtests.jl   | 55 ++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+), 3 deletions(-)

diff --git a/src/orientation.jl b/src/orientation.jl
index 2d9f0edd..5e1c5928 100644
--- a/src/orientation.jl
+++ b/src/orientation.jl
@@ -151,15 +151,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
+    R2 = R2.R
     [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
 
diff --git a/test/runtests.jl b/test/runtests.jl
index e58f7081..b2afa65e 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -542,3 +542,58 @@ using OrdinaryDiffEq
 
     isinteractive() && plot(sol, idxs = [body.r_0...])
 end
+
+# ==============================================================================
+## Four-bar system from https://www.maplesoft.com/documentation_center/online_manuals/modelica/Modelica_Mechanics_MultiBody_Examples_Loops.html#Modelica.Mechanics.MultiBody.Examples.Loops.Fourbar1
+# ==============================================================================
+
+@named begin
+    j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989, isroot = true)
+    j2 = Prismatic(n = [1, 0, 0], s0 = -0.2)
+    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_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 model = ODESystem(connections, t,
+                         systems = [
+                             world,
+                             j1,
+                             j2,
+                             b1,
+                             b2,
+                             b3,
+                             rev,
+                             rev1,
+                             j3,
+                             j4,
+                             j5,
+                             b0,
+                         ])
+# ssys = structural_simplify(IRSystem(model), alias_eliminate = false)
+@test_skip begin # Currentl no handling of kinematic loops
+    ssys = structural_simplify(model, allow_parameters = false)
+
+    prob = ODEProblem(ssys,
+                    [
+                        spring.r_rel_0,
+                    ], (0, 10))
+end
\ No newline at end of file