add use_arrays kwarg

Author: baggepinnen

src/fancy_joints.jl

@@ -377,7 +377,7 @@ end
 
 
 @component function RevoluteWithLengthConstraint(; name, n = Float64[0, 0, 1], axisflange = false,
-    positive_branch = true, radius = 0.05, length = radius, color = [0.5019608f0,0.0f0,0.5019608f0,1.0f0], state_priority = 1.0, phi_offset=0, phi_guess=0, length_constraint=1)
+    positive_branch = true, radius = 0.05, length = radius, color = [0.5019608f0,0.0f0,0.5019608f0,1.0f0], state_priority = 1.0, phi_offset=0, phi_guess=0, length_constraint=1, use_arrays = false)
     systems = @named begin
         frame_a = Frame()
         frame_b = Frame()
@@ -401,11 +401,11 @@ end
         description = "Driving torque in direction of axis of rotation",
     ]
     @variables phi(t) [
-        # state_priority = state_priority,
+        state_priority = 1,
         description = "Relative rotation angle from frame_a to frame_b",
     ]
     @variables angle(t) [
-        # state_priority = -1,
+        state_priority = -1,
         description = "= phi + phi_offset (relative rotation angle between frame_a and frame_b)",
     ]
     @variables r_a(t)[1:3], [description = "Position vector from frame_a to frame_a side of length constraint, resolved in frame_a of revolute joint"]
@@ -443,9 +443,12 @@ end
         0 .~ collect(frame_a.f .+ resolve1(Rrel, frame_b.f))
         0 .~ collect(frame_a.tau .+ resolve1(Rrel, frame_b.tau))
 
-        # angle ~ compute_angle2(length_constraint, e_array, r_a_array, r_b_array, positive_branch)[1]
-        # angle ~ Symbolics.term(compute_angle2, length_constraint, e_array, r_a_array, r_b_array, positive_branch, type=Real)
-        angle ~ compute_angle(length_constraint, e, r_a, r_b, positive_branch)
+        if use_arrays
+            angle ~ compute_angle2(length_constraint, e_array, r_a_array, r_b_array, positive_branch)[1]
+            # angle ~ Symbolics.term(compute_angle2, length_constraint, e_array, r_a_array, r_b_array, positive_branch, type=Real)
+        else
+            angle ~ compute_angle2(length_constraint, e, r_a, r_b, positive_branch)[1]
+        end
     ]
 
     sys = ODESystem(eqs, t; name=:nothing, systems)#, parameter_dependencies = [positive_branch => select_branch(length_constraint, e, phi_offset + phi_guess, r_a, r_b)])  # JuliaSimCompiler ignores parameter dependencies, the user has to provide it instead
@@ -502,6 +505,7 @@ The rest of this joint aggregation is defined by the following parameters:
     phi_offset = 0,
     phi_guess = 0,
     positive_branch,
+    use_arrays = false,
 )
     systems = @named begin
         frame_a = Frame()
@@ -550,6 +554,7 @@ The rest of this joint aggregation is defined by the following parameters:
         phi_offset,
         phi_guess,
         positive_branch,
+        use_arrays,
     )
     push!(more_systems, revolute)
 

test/test_JointUSR_RRR.jl

@@ -16,7 +16,7 @@ W(args...; kwargs...) = Multibody.world
 @mtkmodel TestUSR begin
     @components begin
         world = W()
-        j1 = JointUSR(positive_branch=true)
+        j1 = JointUSR(positive_branch=true, use_arrays=false)
         fixed = FixedTranslation(r=[1,0,0])
         b1 = Body(isroot=false, neg_w=true)
         p1 = Prismatic(state_priority=100)
@@ -32,7 +32,7 @@ end
 @named model = TestUSR()
 model = complete(model)
 ssys = structural_simplify(IRSystem(model))
-prob = ODEProblem(ssys, [model.b1.a_0[1] => 0, D(D(model.p1.s)) => 0], (0.0, 1.0))
+prob = ODEProblem(ssys, [], (0.0, 1.0))
 sol = solve(prob, FBDF(autodiff=true))