diff --git a/src/Multibody.jl b/src/Multibody.jl index 4201f5ee..93b10c37 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -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 diff --git a/src/joints.jl b/src/joints.jl index 8f1640f8..26b93026 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -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() @@ -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() @@ -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 [ @@ -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 \ No newline at end of file diff --git a/src/orientation.jl b/src/orientation.jl index 8fa2f906..cef794ef 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -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"] diff --git a/test/runtests.jl b/test/runtests.jl index 3135142f..759b6235 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1285,4 +1285,30 @@ sol = solve(prob, Rodas4()) include("test_fourbar.jl") end -## ============================================================================= \ No newline at end of file +## ============================================================================= +# 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) \ No newline at end of file