Skip to content

Commit 4776119

Browse files
committed
add some joint assemblies
MWE updates add use_arrays kwarg add broken test add non-broken test
1 parent 4e5ee3a commit 4776119

File tree

7 files changed

+548
-22
lines changed

7 files changed

+548
-22
lines changed

src/Multibody.jl

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
# Find variables that are both array form and scalarized / collected
22
# foreach(println, sort(unknowns(IRSystem(model)), by=string))
33
module Multibody
4-
4+
# Find variables that are both array form and scalarized / collected
5+
# foreach(println, sort(unknowns(IRSystem(model)), by=string))
56
using LinearAlgebra
67
using ModelingToolkit
78
using JuliaSimCompiler

src/components.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -124,7 +124,7 @@ Can be thought of as a massless rod. For a massive rod, see [`BodyShape`](@ref)
124124
@component function FixedTranslation(; name, r, radius=0.02f0, color = purple, render = true)
125125
@named frame_a = Frame()
126126
@named frame_b = Frame()
127-
@parameters r[1:3]=r [
127+
@parameters r[1:3]=collect(r) [
128128
description = "position vector from frame_a to frame_b, resolved in frame_a",
129129
]
130130
r = collect(r)

src/fancy_joints.jl

Lines changed: 414 additions & 5 deletions
Large diffs are not rendered by default.

src/joints.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -617,7 +617,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi
617617

618618
Rrel0 = planar_rotation(n, 0, 0)
619619
varw = false
620-
@named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw)
620+
@named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw, state_priority = -1)
621621

622622
n = collect(n)
623623
ey_a = collect(ey_a)

src/sensors.jl

Lines changed: 39 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,48 @@ function PartialRelativeBaseSensor(; name)
77
equations = [frame_a.f .~ zeros(3) |> collect
88
frame_a.tau .~ zeros(3) |> collect
99
frame_b.f .~ zeros(3) |> collect
10-
frame_b.tau .~ zeros(3) |> collect
11-
frame_resolve.f .~ zeros(3) |> collect
12-
frame_resolve.tau .~ zeros(3) |> collect]
10+
frame_b.tau .~ zeros(3) |> collect]
1311
compose(ODESystem(equations, t; name), frame_a, frame_b)
1412
end
1513

14+
function BasicRelativePosition(; name, resolve_frame)
15+
@named prb = PartialRelativeBaseSensor()
16+
@unpack frame_a, frame_b = prb
17+
systems = @named begin
18+
r_rel = RealOutput(nout = 3)
19+
end
20+
21+
d = collect(frame_b.r_0 - frame_a.r_0)
22+
eqs = if resolve_frame === :frame_a
23+
collect(r_rel.u) .~ resolve2(ori(frame_a), d)
24+
elseif resolve_frame === :frame_b
25+
collect(r_rel.u) .~ resolve2(ori(frame_b), d)
26+
elseif resolve_frame === :world
27+
collect(r_rel.u) .~ d
28+
else
29+
error("resolve_frame must be :world, :frame_a or :frame_b, you provided $resolve_frame, which makes me sad.")
30+
end
31+
32+
extend(compose(ODESystem(eqs, t; name), r_rel), prb)
33+
end
34+
35+
function RelativePosition(; name, resolve_frame = :frame_a)
36+
37+
systems = @named begin
38+
frame_a = Frame()
39+
frame_b = Frame()
40+
relativePosition = BasicRelativePosition(; resolve_frame)
41+
r_rel = RealOutput(nout = 3)
42+
end
43+
44+
eqs = [
45+
connect(relativePosition.frame_a, frame_a)
46+
connect(relativePosition.frame_b, frame_b)
47+
connect(relativePosition.r_rel, r_rel)
48+
]
49+
compose(ODESystem(eqs, t; name), frame_a, frame_b, r_rel, relativePosition)
50+
end
51+
1652
function PartialAbsoluteSensor(; name, n_out)
1753
@named begin
1854
frame_a = Frame()
@@ -82,17 +118,6 @@ function CutForce(; name, resolve_frame = :frame_a)
82118
extend(compose(ODESystem(eqs, t; name), force), pcfbs)
83119
end
84120

85-
function RelativePosition(; name, resolve_frame = :frame_a)
86-
@named begin prs = PartialRelativeBaseSensor(; name) end
87-
88-
@unpack frame_a, frame_b = prs
89-
90-
equations = [frame_a.r_0 .~ frame_b.r_0 |> collect
91-
ori(frame_a) ~ ori(frame_b)
92-
zeros(3) .~ frame_a.r_0 - frame_b.r_0 |> collect]
93-
extend(compose(ODESystem(equations, t; name), frame_a, frame_b), prs)
94-
end
95-
96121
function RelativeAngles(; name, sequence = [1, 2, 3])
97122
@named begin
98123
frame_a = Frame()

test/runtests.jl

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1263,13 +1263,16 @@ sol = solve(prob, Rodas4())
12631263
s = Spherical()
12641264
trans = FixedTranslation(r = [1,0,1])
12651265
body2 = Body(; m = 1, isroot = false, r_cm=[0.1, 0, 0], neg_w=true)
1266+
rp = Multibody.RelativePosition(resolve_frame=:world)
12661267
end
12671268
@equations begin
12681269
connect(world.frame_b, ss.frame_a, trans.frame_a)
12691270
connect(ss.frame_b, ss2.frame_a)
12701271
connect(ss2.frame_b, s.frame_a)
12711272
connect(s.frame_b, trans.frame_b)
12721273
connect(ss.frame_ia, body2.frame_a)
1274+
connect(world.frame_b, rp.frame_a)
1275+
connect(rp.frame_b, ss2.body.frame_a)
12731276
end
12741277
end
12751278

@@ -1282,6 +1285,7 @@ prob = ODEProblem(ssys, [
12821285
], (0, 1.37))
12831286
sol = solve(prob, Rodas4())
12841287
@test SciMLBase.successful_retcode(sol)
1288+
@test sol[collect(model.rp.r_rel.u)] == sol[collect(model.ss.frame_b.r_0)]
12851289
# plot(sol)
12861290

12871291
## =============================================================================

test/test_JointUSR_RRR.jl

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
using Test
2+
using Multibody
3+
using ModelingToolkit
4+
import ModelingToolkitStandardLibrary.Mechanical.Rotational
5+
# using Plots
6+
using OrdinaryDiffEq
7+
using LinearAlgebra
8+
using JuliaSimCompiler
9+
10+
t = Multibody.t
11+
D = Differential(t)
12+
world = Multibody.world
13+
W(args...; kwargs...) = Multibody.world
14+
15+
# A JointUSR is connected to a prismatic joint, with a Body at their common tip
16+
@mtkmodel TestUSR begin
17+
@components begin
18+
world = W()
19+
j1 = JointUSR(positive_branch=true, use_arrays=false)
20+
fixed = FixedTranslation(r=[1,0,0])
21+
b1 = Body(m=1, isroot=false, neg_w=true)
22+
p1 = Prismatic(state_priority=100, n = [1, 0, 0])
23+
end
24+
@equations begin
25+
connect(world.frame_b, j1.frame_a, fixed.frame_a)
26+
connect(fixed.frame_b, p1.frame_a)
27+
connect(p1.frame_b, j1.frame_b)
28+
connect(j1.frame_im, b1.frame_a)
29+
end
30+
end
31+
32+
@named model = TestUSR()
33+
model = complete(model)
34+
ssys = structural_simplify(IRSystem(model))
35+
@test length(unknowns(ssys)) == 2
36+
##
37+
38+
prob = ODEProblem(ssys, [model.p1.v => 0.0], (0.0, 1.4))
39+
sol = solve(prob, FBDF(autodiff=true))
40+
@test sol(1.0, idxs=model.p1.s) -2.8 rtol=0.01 # test vs. OpenModelica
41+
42+
43+
# NOTE: I was working on trying to register the compute_angle function so that there are no symbolic arguments left in the generated code
44+
45+
# foo(x::AbstractArray, p::AbstractArray) = sum(-p .* x .^ 2)
46+
# @register_symbolic foo(x::AbstractArray, p::AbstractArray)
47+
# onetwo = [1, 2]
48+
49+
# @mtkmodel ArrayX begin
50+
# @variables begin
51+
# x(t)[1:2] = ones(2)
52+
# end
53+
# @parameters begin
54+
# p[1:2] = onetwo
55+
# end
56+
# begin
57+
# x = collect(x)
58+
# end
59+
# @equations begin
60+
# D(x[1]) ~ foo(x, p)
61+
# D(x[2]) ~ foo(x, p)
62+
# end
63+
# end
64+
65+
# @named model = ArrayX()
66+
# model = complete(model)
67+
# ssys = structural_simplify(IRSystem(model))
68+
# prob = ODEProblem(ssys, [], (0.0, 1.0))
69+
# sol = solve(prob, FBDF())
70+
71+
72+
# @variables begin
73+
# x(t)[1:2] = 1
74+
# end
75+
# @parameters begin
76+
# p[1:2] = onetwo
77+
# end
78+
# begin
79+
# x = collect(x)
80+
# end
81+
82+
# foo(x, p)
83+
# # @equations begin
84+
# # D(x[1]) ~ foo(x, p)[1]
85+
# # D(x[2]) ~ foo(x, p)[2]
86+
# # end
87+

0 commit comments

Comments
 (0)