Skip to content

Set R to have a negative state priority #116

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/Multibody.jl
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,7 @@ export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, B
include("components.jl")

export Revolute, Prismatic, Planar, Spherical, Universal,
GearConstraint, RollingWheelJoint, RollingWheel, FreeMotion, RevolutePlanarLoopConstraint
GearConstraint, RollingWheelJoint, RollingWheel, FreeMotion, RevolutePlanarLoopConstraint, Cylindrical
include("joints.jl")

export SphericalSpherical, UniversalSpherical, JointUSR, JointRRR
Expand Down
58 changes: 55 additions & 3 deletions src/joints.jl
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotatio
"""
@component function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], axisflange = false,
isroot = true, iscut = false, radius = 0.05, length = radius, color = [0.5019608f0,0.0f0,0.5019608f0,1.0f0], state_priority = 3.0)
if !(eltype(n) <: Num)
if !(eltype(n) <: Num) && !isa(n, Symbolics.Arr{Num, 1})
norm(n) ≈ 1 || error("Axis of rotation must be a unit vector")
end
@named frame_a = Frame()
Expand Down Expand Up @@ -102,8 +102,8 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Transla
The function returns an ODESystem representing the prismatic joint.
"""
@component function Prismatic(; name, n = Float64[0, 0, 1], axisflange = false,
s0 = 0, v0 = 0, radius = 0.05, color = [0,0.8,1,1], state_priority=10, iscut=false)
if !(eltype(n) <: Num)
s0 = 0, v0 = 0, radius = 0.05, color = [0,0.8,1,1], state_priority=10, iscut=false, render=true)
if !(eltype(n) <: Num) && !isa(n, Symbolics.Arr{Num, 1})
norm(n) ≈ 1 || error("Prismatic axis of motion must be a unit vector, got norm(n) = $(norm(n))")
end
@named frame_a = Frame()
Expand All @@ -114,6 +114,7 @@ The function returns an ODESystem representing the prismatic joint.
pars = @parameters begin
radius = radius, [description = "radius of the joint in animations"]
color[1:4] = color, [description = "color of the joint in animations (RGBA)"]
render = render, [description = "render the joint in animations"]
end

@variables s(t)=s0 [
Expand Down Expand Up @@ -987,3 +988,54 @@ s_y=prismatic_y.s=0` and `phi=revolute.phi=0`.
connect(revolute.frame_b, frame_b)
end
end

@mtkmodel Cylindrical begin
begin
n_def = [1, 0, 0] # Workaround for mtkmodel bug
cylinder_color_def = [1, 0, 1, 1]
end

@structural_parameters begin
# _state_priority = 2 # mtkmodel bug prevents this from being any form of parameter at all :/
cylinder_color = [1, 0, 1, 1]#, [description = "Color of cylinder"]
end

@parameters begin
n[1:3] = n_def, [description = "Cylinder axis resolved in frame_a (= same as in frame_b)"]
cylinder_diameter = 0.05, [description = "Diameter of cylinder"]
render = true, [description = "Enable rendering of the joint in animations"]
end
begin
n = collect(n)
cylinder_color = collect(cylinder_color)
end

@components begin
frame_a = Frame()
frame_b = Frame()
prismatic = Prismatic(; n, state_priority=0, render = false)
revolute = Revolute(; n, state_priority=0, color = cylinder_color, radius = cylinder_diameter/2)
end

@variables begin
(s(t) = 0), [state_priority = 200, description = "Relative distance between frame_a and frame_b"]
(phi(t) = 0), [state_priority = 200, description = "Relative rotation angle from frame_a to frame_b"]
(v(t) = 0), [state_priority = 200, description = "First derivative of s (relative velocity)"]
(w(t) = 0), [state_priority = 200, description = "First derivative of angle phi (relative angular velocity)"]
(a(t) = 0), [description = "Second derivative of s (relative acceleration)"]
(wd(t) = 0), [description = "Second derivative of angle phi (relative angular acceleration)"]
end

@equations begin
phi ~ revolute.phi
w ~ D(phi)
wd ~ D(w)
s ~ prismatic.s
v ~ D(s)
a ~ D(v)
connect(frame_a, prismatic.frame_a)
connect(prismatic.frame_b, revolute.frame_a)
connect(revolute.frame_b, frame_b)
end

end
2 changes: 1 addition & 1 deletion src/orientation.jl
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ The primary difference between `NumRotationMatrix` and `RotationMatrix` is that

Never call this function directly from a component constructor, instead call `f = Frame(); R = ori(f)` and add `f` to the subsystems.
"""
function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name=:R, varw = false, state_priority=nothing)
function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name=:R, varw = false, state_priority=-1)
# The reason for not calling this directly is that all R vaiables have to have the same name since they are treated as connector variables (otherwise a connection error is thrown). A component with more than one rotation matrix will thus have two different R variables that overwrite each other
R = at_variables_t(:R, 1:3, 1:3; default = R, state_priority) #[description="Orientation rotation matrix ∈ SO(3)"]
# @variables w(t)[1:3]=w [description="angular velocity"]
Expand Down
28 changes: 27 additions & 1 deletion test/runtests.jl
Original file line number Diff line number Diff line change
Expand Up @@ -1285,4 +1285,30 @@ sol = solve(prob, Rodas4())
include("test_fourbar.jl")
end

## =============================================================================
## =============================================================================
# using Plots
# Test cylindrical joint
@mtkmodel CylinderTest begin
@components begin
world = W()
cyl = Cylindrical(n = [0, 1, 0])
# spring = Spring(c = 1)
body = Body(state_priority=0)
end
@equations begin
# connect(world.frame_b, cyl.frame_a, spring.frame_a)
# connect(cyl.frame_b, spring.frame_b, body.frame_a)

connect(world.frame_b, cyl.frame_a)
connect(cyl.frame_b, body.frame_a)
end
end
@named model = CylinderTest()
model = complete(model)
ssys = structural_simplify(IRSystem(model))
prob = ODEProblem(ssys, [
model.body.v_0[1] => 1
model.body.r_0[1] => 0.1
], (0, 1))
sol = solve(prob, Rodas4())
plot(sol)
Loading