add RevolutePlanarLoopConstraint

examples/fourbar.jl

@@ -2,6 +2,7 @@ using Multibody, OrdinaryDiffEq, JuliaSimCompiler
 using ModelingToolkit
 using Multibody: connect_loop
 using Test
+using Plots
 t = Multibody.t
 
 world = Multibody.world
@@ -59,24 +60,29 @@ W(args...; kwargs...) = Multibody.world
 # 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)
+systems = @named begin
+    j1 = Revolute(n = [1, 0, 0], w0 = 5.235987755982989)
+    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])
+    j3 = Revolute(n = [1, 0, 0], iscut=true)
+    # j3 = RevolutePlanarLoopConstraint(n = [1.0, 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)
+
+            #    Multibody.connect_loop(j1.frame_b, b1.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)
+
+            #    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)
@@ -85,29 +91,43 @@ connections = [connect(j2.frame_b, b2.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,
-                         ])
+               connect(b0.frame_b, j2.frame_a)
+               ]
+@named fourbar = ODESystem(connections, t, systems = [world; systems])
 
 # 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.())
+du = zero(prob.u0)
+prob.f(du, prob.u0, prob.p, 0.0) 
+
+@test_skip begin
+    sol = solve(prob, Rodas4(autodiff=true), u0 = prob.u0 .+ 0.000001 .* randn.())
+    @test SciMLBase.successful_retcode(sol)
+    # plot(sol); hline!([-pi pi], l=(:dash, :black)) # NOTE: it looks like we hit a singularity in the orientation representation since the simulation dies when some angles get close to ±π, but we do not have any Euler angles as states so I'm not sure why that is
+end
+
+# using SeeToDee, NonlinearSolve
+# function dynamics(x,u,p,t)
+#     dx = similar(x)
+#     prob.f(dx,x,p,t)
+# end
+
+# Ts = 0.001
+# nx = 2
+# na = 7
+# nu = 0
+# x_inds = findall(!iszero, prob.f.mass_matrix.diag)
+# a_inds = findall(iszero, prob.f.mass_matrix.diag)
+# discrete_dynamics = SeeToDee.SimpleColloc2(dynamics, Ts, x_inds, a_inds, nu; n=3, solver=NonlinearSolve.NewtonRaphson())
+
+# X = [prob.u0]
+# i = 0
+# for i in 1:1000
+#     push!(X, discrete_dynamics(X[end], [], prob.p, i*Ts))
+# end
 
 
 
@@ -155,44 +175,69 @@ isinteractive() && plot(sol)
 # render(fourbar, sol; framerate=60)
 
 ## 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
+# We must also replace one joint with a RevolutePlanarLoopConstraint 
 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])
+    j1 = Revolute(useAxisFlange=true)
+    j2 = Revolute()
+    j3 = Revolute()
+    # j4 = Revolute()
+    # j2 = RevolutePlanarLoopConstraint()
+    # j3 = RevolutePlanarLoopConstraint()
+    j4 = RevolutePlanarLoopConstraint()
+    j5 = Revolute()
+    b1 = BodyShape(m=1, r = [1.0, 0, 0], radius = 1*0.08)
+    b2 = BodyShape(m=1, r = [0.0, 1.0, 0], radius = 1.2*0.08)
+    b3 = BodyShape(m=1, r = [-1.0, 0, 0], radius = 1.4*0.08)
+    b4 = BodyShape(m=1, r = [0.0, -1.0, 0], radius = 1.6*0.08)
+    damper1 = Rotational.Damper(d=1)
+    damper2 = Rotational.Damper(d=1)
 end
 
 connections = [
-    # connect(world.frame_b, j1.frame_a)
+    connect(world.frame_b, j1.frame_a)
+    # Multibody.connect_loop(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)
+    # Multibody.connect_loop(j4.frame_b, b4.frame_a)
+
+    connect(b4.frame_b, j5.frame_a)
+    connect(j5.frame_b, world.frame_b) # Attached to world
+    # NOTE: we need 5 joints since j1.frame_a is rigidly attached to the world, and b4 closing the loop can thus not rotate around j1. One could potentially avoid connecting j1 to world and instead just force the positional coordinates of j1 to be zero.
+    # Multibody.connect_loop(b4.frame_b, j1.frame_a)
+
+    connect(j1.axis, damper1.flange_a)
+    connect(j1.support, damper1.flange_b)
+
+    connect(j2.axis, damper2.flange_a)
+    connect(j2.support, damper2.flange_b)
+
 ]
 @named fourbar = ODESystem(connections, t, systems = [world; systems])
 
-m = structural_simplify(IRSystem(fourbar))
+#
+
+# m = structural_simplify((fourbar), allow_parameter=false)
+m = structural_simplify(IRSystem(fourbar)) # It does simplify
 
 @test_broken length(states(m)) == 2
 
-prob = ODEProblem(m, [], (0.0, 5.0))
+prob = ODEProblem(m, [], (0.0, 12.0))
 
 # Try the generated dynamics
 du = zero(prob.u0)
-prob.f.f(du, prob.u0, prob.p, 0)
+prob.f.f(du, prob.u0 .+ 0.0001 .* randn.(), 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])
+sol = solve(prob, Rodas4(autodiff=false))
+@test SciMLBase.successful_retcode(sol)
+@test sol(sol.t[end], idxs=b2.frame_b.r_0[2]) < -1.5 # Test the the "pendulum" is hanging almsot straight down after sufficient time has passed
+# isinteractive() && plot(sol, idxs = [j1.phi, j2.phi, j3.phi])
+# first(render(fourbar, sol, 0.1, z=-5))
+# render(fourbar, sol, z=-5, framerate=30, R=Multibody.rotx(20, true)*Multibody.roty(20, true))

src/Multibody.jl

@@ -134,7 +134,7 @@ export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, B
 include("components.jl")
 
 export Revolute, Prismatic, Spherical, Universal, GearConstraint, RollingWheelJoint,
-       RollingWheel, FreeMotion
+       RollingWheel, FreeMotion, RevolutePlanarLoopConstraint
 include("joints.jl")
 
 export Spring, Damper, SpringDamperParallel, Torque, Force

src/joints.jl

@@ -14,7 +14,7 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota
 - `support`: 1-dim. rotational flange of the drive support (assumed to be fixed in the world frame, NOT in the joint)
 """
 @component function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], useAxisFlange = false,
-                  isroot = true, radius = 0.1)
+                  isroot = true, iscut = false, radius = 0.1)
     norm(n) ≈ 1 || error("Axis of rotation must be a unit vector")
     @named frame_a = Frame()
     @named frame_b = Frame()
@@ -34,16 +34,23 @@ If `useAxisFlange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rota
     @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w)
     n = collect(n)
 
-    if isroot
-        eqs = Equation[Rrel ~ planar_rotation(n, phi, w)
-                       ori(frame_b) ~ absoluteRotation(ori(frame_a), Rrel)
-                       collect(frame_a.f) .~ -resolve1(Rrel, frame_b.f)
-                       collect(frame_a.tau) .~ -resolve1(Rrel, frame_b.tau)]
-    else
+    if iscut
         eqs = Equation[Rrel ~ planar_rotation(-n, phi, w)
-                       ori(frame_a) ~ absoluteRotation(ori(frame_b), Rrel)
-                       collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f)
-                       collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)]
+            residue(ori(frame_a), absoluteRotation(ori(frame_b), Rrel)) .~ 0 # If joint is a cut joint, this equation is replaced
+                collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f)
+                collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)]
+    else
+        if isroot
+            eqs = Equation[Rrel ~ planar_rotation(n, phi, w)
+                        ori(frame_b) ~ absoluteRotation(ori(frame_a), Rrel)
+                        collect(frame_a.f) .~ -resolve1(Rrel, frame_b.f)
+                        collect(frame_a.tau) .~ -resolve1(Rrel, frame_b.tau)]
+        else
+            eqs = Equation[Rrel ~ planar_rotation(-n, phi, w)
+                        ori(frame_a) ~ absoluteRotation(ori(frame_b), Rrel)
+                        collect(frame_b.f) .~ -resolve1(Rrel, frame_a.f)
+                        collect(frame_b.tau) .~ -resolve1(Rrel, frame_a.tau)]
+        end
     end
     moreeqs = [collect(frame_a.r_0 .~ frame_b.r_0)
                D(phi) ~ w
@@ -122,7 +129,7 @@ The function returns an ODESystem representing the prismatic joint.
            zeros(3) .~ collect(frame_a.tau + frame_b.tau + cross(n * s, frame_b.f))
 
            # d'Alemberts principle
-           f .~ -n'collect(frame_b.f)]
+           f ~ -(n'collect(frame_b.f))[]]
 
     if useAxisFlange
         @named fixed = Translational.Fixed(s0=0)
@@ -708,3 +715,71 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
     end
     compose(ODESystem(eqs, t; name), frame_a, frame_b)
 end
+
+"""
+    RevolutePlanarLoopConstraint(; name, n)
+
+Revolute joint that is described by 2 positional constraints for usage in a planar loop (the ambiguous cut-force perpendicular to the loop and the ambiguous cut-torques are set arbitrarily to zero)
+
+Joint where `frame_b` rotates around axis `n` which is fixed in `frame_a` and where this joint is used in a planar loop providing 2 constraint equations on position level.
+
+If a planar loop is present, e.g., consisting of 4 revolute joints where the joint axes are all parallel to each other, then there is no unique mathematical solution if all revolute joints are modelled with `Revolute` and the symbolic algorithms will fail. The reason is that, e.g., the cut-forces in the revolute joints perpendicular to the planar loop are not uniquely defined when 3-dim. descriptions of revolute joints are used. Usually, an error message will be printed pointing out this situation. In this case, one revolute joint in the loop has to be replaced by model `RevolutePlanarLoopCutJoint`. The effect is that from the 5 constraints of a 3-dim. revolute joint, 3 constraints are removed and replaced by appropriate known variables (e.g., the force in the direction of the axis of rotation is treated as known with value equal to zero; for standard revolute joints, this force is an unknown quantity).
+"""
+@component function RevolutePlanarLoopConstraint(; name, n = Float64[0, 0, 1])
+    norm(n) ≈ 1 || error("Axis of rotation must be a unit vector")
+    @named frame_a = Frame()
+    @named frame_b = Frame()
+
+    # n isa Vector{Float64} || error("Parametric axis of rotation is currently not supported")
+
+    # Activate this when symbolic parameters are a bit more robust
+    # @parameters n[1:3]=n [description = "axis of rotation"]
+
+    # # @parameters e[1:3] [description = "Unit vector in direction of rotation axis, resolved in frame_a (= same as in frame_b)"]
+    # @parameters nnx_a[1:3] = [1,0,0]#ifelse(abs(n[1]) > 0.1, [0,1,0], ifelse(abs(n[2]) > 0.1, [0,0,1], [1,0,0])) [description = "Arbitrary vector that is not aligned with rotation axis n"]
+    # @parameters ey_a[1:3] = [0,1,0]#inormalize(cross(n, nnx_a)) [description = "Unit vector orthogonal to axis n of revolute joint, resolved in frame_a"]
+    # @parameters ex_a[1:3] = [1,0,0]#icross(ey_a, n) [description = "Unit vector orthogonal to axis n of revolute joint and to ey_a, resolved in frame_a"]
+    # # @variables ey_b[1:3](t) [description = "ey_a, resolved in frame_b"]
+    # # @variables ex_b[1:3](t) [description = "ex_a, resolved in frame_b"]
+
+
+
+    nnx_a = ifelse(abs(n[1]) > 0.1, [0,1,0], ifelse(abs(n[2]) > 0.1, [0,0,1], [1,0,0])) 
+    ey_a = normalize(cross(n, nnx_a)) 
+    ex_a = cross(ey_a, n) 
+
+
+    @variables r_rel_a(t)[1:3] [description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a"]
+    @variables f_c(t)[1:2] [description = "Dummy or constraint forces in direction of ex_a, ey_a"]
+
+
+    # @named R_rel = NumRotationMatrix()
+
+    Rrel0 = planar_rotation(n, 0, 0)
+    varw = false
+    @named R_rel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw)
+
+    n = collect(n)
+    ey_a = collect(ey_a)
+    ex_a = collect(ex_a)
+    r_rel_a = collect(r_rel_a)
+    f_c = collect(f_c)
+
+    Rb = ori(frame_b)
+
+    eqs = [
+        R_rel ~ relativeRotation(ori(frame_a), ori(frame_b))
+        r_rel_a .~ resolve2(ori(frame_a), collect(frame_b.r_0 - frame_a.r_0))
+        0 ~ (ex_a'r_rel_a)[]
+        0 ~ (ey_a'r_rel_a)[]
+        collect(frame_a.tau) .~ zeros(3)
+        collect(frame_b.tau) .~ zeros(3)
+        collect(frame_a.f) .~ vec([ex_a ey_a]*f_c)
+        collect(frame_b.f) .~ -resolve2(R_rel, frame_a.f)
+
+    ]
+    compose(ODESystem(eqs, t; name), frame_a, frame_b)
+end
+
+LinearAlgebra.normalize(a::Vector{Num}) = a / norm(a)
+

src/orientation.jl

@@ -210,16 +210,23 @@ function residue(R1, R2)
     # https://github.com/modelica/ModelicaStandardLibrary/blob/master/Modelica/Mechanics/MultiBody/Frames/Orientation.mo
     R1 isa ODESystem && (R1 = ori(R1))
     R2 isa ODESystem && (R2 = ori(R2))
-    R1 = R1.R.mat
-    R2 = R2.R.mat
+    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, :])]
+
+    # [
+    # cross(R1[1, :], R1[2, :])'R2[2, :]
+    # -cross(R1[1, :], R1[2, :])'R2[1, :]
+    # R1[2, :]'R2[1, :]
+    # ]
 end
 
 function connect_loop(F1, F2)
     F1.metadata[:loop_opening] = true
     # connect(F1, F2)
+    # orientation_constraint(ori(F1)'ori(F2)) .~ 0
     residue(F1, F2) .~ 0
 end