add test for JointRRR

change order of operations

Author: baggepinnen

src/fancy_joints.jl

@@ -519,7 +519,7 @@ The rest of this joint aggregation is defined by the following parameters:
     end
 
     @parameters begin
-        n1_a[1:3] = n1_a, [description = "Axis 1 of universal joint fixed and resolved in frame_a (axis 2 is orthogonal to axis 1 and to rod 1)"]
+        # n1_a[1:3] = n1_a, [description = "Axis 1 of universal joint fixed and resolved in frame_a (axis 2 is orthogonal to axis 1 and to rod 1)"]
         # n_b[1:3] = n_b, [description = "Axis of revolute joint fixed and resolved in frame_b"]
         rRod1_ia[1:3] = rRod1_ia, [description = "Vector from origin of frame_a to spherical joint, resolved in frame_ia"]
         # rRod2_ib[1:3] = rRod2_ib, [description = "Vector from origin of frame_ib to spherical joint, resolved in frame_ib"]
@@ -648,15 +648,15 @@ Basically, the JointRRR model internally consists of a universal-spherical-revol
     rRod2_ib = [-1,0,0],
     phi_offset = 0, 
     phi_guess = 0,
-
+    positive_branch = true,
 )
 
     @parameters begin
-        n_a[1:3] = n_a, [description = "Axes of revolute joints resolved in frame_a (all axes are parallel to each other)"]
-        n_b[1:3] = n_b, [description = "Axis of revolute joint fixed and resolved in frame_b"]
+        # n_a[1:3] = n_a, [description = "Axes of revolute joints resolved in frame_a (all axes are parallel to each other)"]
+        # n_b[1:3] = n_b, [description = "Axis of revolute joint fixed and resolved in frame_b"]
         rRod1_ia[1:3] = rRod1_ia, [description = "Vector from origin of frame_a to revolute joint in the middle, resolved in frame_ia"]
-        rRod2_ib[1:3] = rRod2_ib, [description = "Vector from origin of frame_ib to revolute joint in the middle, resolved in frame_ib"]
-        phi_offset = phi_offset, [description = "Relative angle offset of revolute joint at frame_b (angle = phi(t) + from_deg(phi_offset))"]
+        # rRod2_ib[1:3] = rRod2_ib, [description = "Vector from origin of frame_ib to revolute joint in the middle, resolved in frame_ib"]
+        # phi_offset = phi_offset, [description = "Relative angle offset of revolute joint at frame_b (angle = phi(t) + from_deg(phi_offset))"]
         phi_guess = phi_guess, [description = "Select the configuration such that at initial time |phi(t0) - from_deg(phi_guess)| is minimal"]
 
     end
@@ -675,6 +675,7 @@ Basically, the JointRRR model internally consists of a universal-spherical-revol
             phi_guess,
             rRod2_ib,
             rRod1_ia,
+            positive_branch
         )
     end
 

test/runtests.jl

@@ -1323,3 +1323,9 @@ sol = solve(prob, Rodas4())
 @test sol[model.cyl.v][end] ≈ -9.81 atol=0.01
 @test sol[model.cyl.phi][end] ≈ 1 atol=0.01
 
+## =============================================================================
+
+@testset "JointUSR_RRR" begin
+    @info "Testing JointUSR_RRR"
+    include("test_JointUSR_RRR.jl")
+end

test/test_JointUSR_RRR.jl

@@ -7,18 +7,20 @@ using OrdinaryDiffEq
 using LinearAlgebra
 using JuliaSimCompiler
 
-t = Multibody.t
-D = Differential(t)
 world = Multibody.world
 W(args...; kwargs...) = Multibody.world
+t = Multibody.t
+D = Differential(t)
 
-# A JointUSR is connected to a prismatic joint, with a Body at their common tip
+# ==============================================================================
+## 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)
+        b1 = Body(m=1)
         p1 = Prismatic(state_priority=100, n = [1, 0, 0])
     end
     @equations begin
@@ -40,48 +42,33 @@ 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())
 
+# ==============================================================================
+## A JointRRR is connected to a prismatic joint, with a Body at their common tip
+# ==============================================================================
+@mtkmodel TestRRR begin
+    @components begin
+        world = W()
+        j1 = JointRRR(positive_branch=true)
+        fixed = FixedTranslation(r=[1,0,0])
+        b1 = Body(m=1)
+        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
 
-# @variables begin
-#     x(t)[1:2] = 1
-# end
-# @parameters begin
-#     p[1:2] = onetwo
-# end
-# begin
-#     x = collect(x)
-# end
+@named model = TestRRR()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+@test length(unknowns(ssys)) == 2
+##
 
-# foo(x, p)
-# # @equations begin
-# #     D(x[1]) ~ foo(x, p)[1]
-# #     D(x[2]) ~ foo(x, p)[2]
-# # end
+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