add some joint assemblies

MWE

updates

add use_arrays kwarg

add broken test

add non-broken test

Author: baggepinnen

src/Multibody.jl

@@ -1,7 +1,8 @@
 # Find variables that are both array form and scalarized / collected
 # foreach(println, sort(unknowns(IRSystem(model)), by=string))
 module Multibody
-
+# Find variables that are both array form and scalarized / collected
+# foreach(println, sort(unknowns(IRSystem(model)), by=string))
 using LinearAlgebra
 using ModelingToolkit
 using JuliaSimCompiler

src/components.jl

@@ -124,7 +124,7 @@ Can be thought of as a massless rod. For a massive rod, see [`BodyShape`](@ref)
 @component function FixedTranslation(; name, r, radius=0.02f0, color = purple, render = true)
     @named frame_a = Frame()
     @named frame_b = Frame()
-    @parameters r[1:3]=r [
+    @parameters r[1:3]=collect(r) [
         description = "position vector from frame_a to frame_b, resolved in frame_a",
     ]
     r = collect(r)

src/fancy_joints.jl

@@ -617,7 +617,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi
 
     Rrel0 = planar_rotation(n, 0, 0)
     varw = false
-    @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw)
+    @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw, state_priority = -1)
 
     n = collect(n)
     ey_a = collect(ey_a)

src/joints.jl

@@ -7,12 +7,48 @@ function PartialRelativeBaseSensor(; name)
     equations = [frame_a.f .~ zeros(3) |> collect
                  frame_a.tau .~ zeros(3) |> collect
                  frame_b.f .~ zeros(3) |> collect
-                 frame_b.tau .~ zeros(3) |> collect
-                 frame_resolve.f .~ zeros(3) |> collect
-                 frame_resolve.tau .~ zeros(3) |> collect]
+                 frame_b.tau .~ zeros(3) |> collect]
     compose(ODESystem(equations, t; name), frame_a, frame_b)
 end
 
+function BasicRelativePosition(; name, resolve_frame)
+    @named prb = PartialRelativeBaseSensor()
+    @unpack frame_a, frame_b = prb
+    systems = @named begin
+        r_rel = RealOutput(nout = 3)
+    end
+
+    d = collect(frame_b.r_0 - frame_a.r_0)
+    eqs = if resolve_frame === :frame_a
+        collect(r_rel.u) .~ resolve2(ori(frame_a), d)
+    elseif resolve_frame === :frame_b
+        collect(r_rel.u) .~ resolve2(ori(frame_b), d)
+    elseif resolve_frame === :world
+        collect(r_rel.u) .~ d
+    else
+        error("resolve_frame must be :world, :frame_a or :frame_b, you provided $resolve_frame, which makes me sad.")
+    end
+
+    extend(compose(ODESystem(eqs, t; name), r_rel), prb)
+end
+
+function RelativePosition(; name, resolve_frame = :frame_a)
+
+    systems = @named begin
+        frame_a = Frame()
+        frame_b = Frame()
+        relativePosition = BasicRelativePosition(; resolve_frame)
+        r_rel = RealOutput(nout = 3)
+    end
+
+    eqs = [
+        connect(relativePosition.frame_a, frame_a)
+        connect(relativePosition.frame_b, frame_b)
+        connect(relativePosition.r_rel, r_rel)
+    ]
+    compose(ODESystem(eqs, t; name), frame_a, frame_b, r_rel, relativePosition)
+end
+
 function PartialAbsoluteSensor(; name, n_out)
     @named begin
         frame_a = Frame()
@@ -82,17 +118,6 @@ function CutForce(; name, resolve_frame = :frame_a)
     extend(compose(ODESystem(eqs, t; name), force), pcfbs)
 end
 
-function RelativePosition(; name, resolve_frame = :frame_a)
-    @named begin prs = PartialRelativeBaseSensor(; name) end
-
-    @unpack frame_a, frame_b = prs
-
-    equations = [frame_a.r_0 .~ frame_b.r_0 |> collect
-                 ori(frame_a) ~ ori(frame_b)
-                 zeros(3) .~ frame_a.r_0 - frame_b.r_0 |> collect]
-    extend(compose(ODESystem(equations, t; name), frame_a, frame_b), prs)
-end
-
 function RelativeAngles(; name, sequence = [1, 2, 3])
     @named begin
         frame_a = Frame()

src/sensors.jl

@@ -1263,13 +1263,16 @@ sol = solve(prob, Rodas4())
         s = Spherical()
         trans = FixedTranslation(r = [1,0,1])
         body2 = Body(; m = 1, isroot = false, r_cm=[0.1, 0, 0], neg_w=true)
+        rp = Multibody.RelativePosition(resolve_frame=:world)
     end
     @equations begin
         connect(world.frame_b, ss.frame_a, trans.frame_a)
         connect(ss.frame_b, ss2.frame_a)
         connect(ss2.frame_b, s.frame_a)
         connect(s.frame_b, trans.frame_b)
         connect(ss.frame_ia, body2.frame_a)
+        connect(world.frame_b, rp.frame_a)
+        connect(rp.frame_b, ss2.body.frame_a)
     end
 end
 
@@ -1282,6 +1285,7 @@ prob = ODEProblem(ssys, [
 ], (0, 1.37))
 sol = solve(prob, Rodas4())
 @test SciMLBase.successful_retcode(sol)
+@test sol[collect(model.rp.r_rel.u)] == sol[collect(model.ss.frame_b.r_0)]
 # plot(sol)
 
 ## =============================================================================

test/runtests.jl

@@ -0,0 +1,87 @@
+using Test
+using Multibody
+using ModelingToolkit
+import ModelingToolkitStandardLibrary.Mechanical.Rotational
+# using Plots
+using OrdinaryDiffEq
+using LinearAlgebra
+using JuliaSimCompiler
+
+t = Multibody.t
+D = Differential(t)
+world = Multibody.world
+W(args...; kwargs...) = Multibody.world
+
+# A JointUSR is connected to a prismatic joint, with a Body at their common tip
+@mtkmodel TestUSR begin
+    @components begin
+        world = W()
+        j1 = JointUSR(positive_branch=true, use_arrays=false)
+        fixed = FixedTranslation(r=[1,0,0])
+        b1 = Body(m=1, isroot=false, neg_w=true)
+        p1 = Prismatic(state_priority=100, n = [1, 0, 0])
+    end
+    @equations begin
+        connect(world.frame_b, j1.frame_a, fixed.frame_a)
+        connect(fixed.frame_b, p1.frame_a)
+        connect(p1.frame_b, j1.frame_b)
+        connect(j1.frame_im, b1.frame_a)
+    end
+end
+
+@named model = TestUSR()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+@test length(unknowns(ssys)) == 2
+##
+
+prob = ODEProblem(ssys, [model.p1.v => 0.0], (0.0, 1.4))
+sol = solve(prob, FBDF(autodiff=true))
+@test sol(1.0, idxs=model.p1.s) ≈ -2.8 rtol=0.01 # test vs. OpenModelica
+
+
+# NOTE: I was working on trying to register the compute_angle function so that there are no symbolic arguments left in the generated code
+
+# foo(x::AbstractArray, p::AbstractArray) = sum(-p .* x .^ 2)
+# @register_symbolic foo(x::AbstractArray, p::AbstractArray)
+# onetwo = [1, 2]
+
+# @mtkmodel ArrayX begin
+#     @variables begin
+#         x(t)[1:2] = ones(2)
+#     end
+#     @parameters begin
+#         p[1:2] = onetwo
+#     end
+#     begin
+#         x = collect(x)
+#     end
+#     @equations begin
+#         D(x[1]) ~ foo(x, p)
+#         D(x[2]) ~ foo(x, p)
+#     end
+# end
+
+# @named model = ArrayX()
+# model = complete(model)
+# ssys = structural_simplify(IRSystem(model))
+# prob = ODEProblem(ssys, [], (0.0, 1.0))
+# sol = solve(prob, FBDF())
+
+
+# @variables begin
+#     x(t)[1:2] = 1
+# end
+# @parameters begin
+#     p[1:2] = onetwo
+# end
+# begin
+#     x = collect(x)
+# end
+
+# foo(x, p)
+# # @equations begin
+# #     D(x[1]) ~ foo(x, p)[1]
+# #     D(x[2]) ~ foo(x, p)[2]
+# # end
+