Add JointUSR and JointRRR

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/PlanarMechanics/sensors.jl

@@ -294,9 +294,7 @@ Measure relative position and orientation between the origins of two frame conne
  @named fr = FrameResolve()
  push!(systems, fr)
  push!(eqs, connect(pos.frame_resolve, fr))
- end
-
- if resolve_in_frame != :frame_resolve
+ else
  @named zero_position = ZeroPosition()
  push!(systems, zero_position)
  push!(eqs, connect(zero_position.frame_resolve, pos.frame_resolve))

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/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/sensors.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()

test/runtests.jl

@@ -1175,6 +1175,7 @@ prob = ODEProblem(ssys, [
 
 ], (0, 1))
 sol1 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8)
+@test SciMLBase.successful_retcode(sol1)
 
 ## quat in joint
 @named joint = Multibody.Spherical(isroot=true, state=true, quat=true, neg_w=true)
@@ -1195,6 +1196,7 @@ prob = ODEProblem(ssys, [
 
 ], (0, 1))
 sol2 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8)
+@test SciMLBase.successful_retcode(sol2)
 
 ## euler
 @named joint = Multibody.Spherical(isroot=true, state=true, quat=false, neg_w=true)
@@ -1259,17 +1261,20 @@ sol = solve(prob, Rodas4())
  @components begin
  world = W()
  ss = UniversalSpherical(rRod_ia = [1, 0, 0], kinematic_constraint=false, sphere_diameter=0.3)
- ss2 = BodyShape(r = [0, 0, 1], m = 1, isroot=true, neg_w=true)
+ ss2 = BodyShape(r = [0, 0, 1], m = 1, isroot=true)
  s = Spherical()
  trans = FixedTranslation(r = [1,0,1])
- body2 = Body(; m = 1, isroot = false, r_cm=[0.1, 0, 0], neg_w=true)
+ body2 = Body(; m = 1, r_cm=[0.1, 0, 0])
+ # 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
 
@@ -1278,10 +1283,12 @@ model = complete(model)
 ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [
  model.ss2.body.phi[1] => 0.1;
+ model.ss2.body.phi[3] => 0.1;
  model.ss2.body.phid[3] => 0.0;
 ], (0, 1.37))
 sol = solve(prob, Rodas4())
 @test SciMLBase.successful_retcode(sol)
+@test_skip sol[collect(model.rp.r_rel.u)] == sol[collect(model.ss.frame_b.r_0)] # This test is commented out, adding the sensor makes the problem singular and I can't seem to find a set of state priorities that make it solve
 # plot(sol)
 
 ## =============================================================================
@@ -1319,3 +1326,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

@@ -0,0 +1,74 @@
+using Test
+using Multibody
+using ModelingToolkit
+import ModelingToolkitStandardLibrary.Mechanical.Rotational
+# using Plots
+using OrdinaryDiffEq
+using LinearAlgebra
+using JuliaSimCompiler
+
+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
+# ==============================================================================
+@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)
+ 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
+
+
+
+# ==============================================================================
+## 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
+
+@named model = TestRRR()
+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
+