From d9ec5bf7f83e3eb8310d90a504738164abfd73df Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 8 Aug 2024 12:24:51 +0200 Subject: [PATCH] add use_arrays kwarg --- src/fancy_joints.jl | 17 +++++++++++------ test/test_JointUSR_RRR.jl | 4 ++-- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/fancy_joints.jl b/src/fancy_joints.jl index fc46d03e..3add3131 100644 --- a/src/fancy_joints.jl +++ b/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) diff --git a/test/test_JointUSR_RRR.jl b/test/test_JointUSR_RRR.jl index 74d51bc6..2c6b8667 100644 --- a/test/test_JointUSR_RRR.jl +++ b/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))