From 44dc7ca330ed6c2b0c3a81ca6304f8e3b8571c9a Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 4 Jul 2024 13:45:00 +0200 Subject: [PATCH] update Universal and Spherical rendering --- docs/src/examples/spherical_pendulum.md | 4 +-- docs/src/rendering.md | 6 ++-- ext/Render.jl | 15 ++++++--- src/joints.jl | 44 +++++++++++++++++-------- test/runtests.jl | 15 +++------ 5 files changed, 51 insertions(+), 33 deletions(-) diff --git a/docs/src/examples/spherical_pendulum.md b/docs/src/examples/spherical_pendulum.md index 1a717ab0..b15764ed 100644 --- a/docs/src/examples/spherical_pendulum.md +++ b/docs/src/examples/spherical_pendulum.md @@ -17,9 +17,9 @@ D = Differential(t) world = Multibody.world systems = @named begin - joint = Spherical(state=true, isroot=true, phi = 1, radius=0.2, color=[1,1,0,1]) + joint = Spherical(state=true, isroot=true, phi = 1, phi_d = 3, radius=0.1, color=[1,1,0,1]) bar = FixedTranslation(r = [0, -1, 0]) - body = Body(; m = 1, isroot = false) + body = Body(; m = 1, isroot = false, r_cm=[0.1, 0, 0]) end connections = [connect(world.frame_b, joint.frame_a) diff --git a/docs/src/rendering.md b/docs/src/rendering.md index b830339f..a3b632bb 100644 --- a/docs/src/rendering.md +++ b/docs/src/rendering.md @@ -2,16 +2,16 @@ Multibody.jl has an automatic 3D-rendering feature that draws a mechanism in 3D. This can be used to create animations of the mechanism's motion from a solution trajectory, as well as to create interactive applications where the evolution of time can be controlled by the user. -The functionality requires the user to load any of the Makie frontend packages, e.g., +The functionality requires the user to install and load one of the [Makie backend packages](https://docs.makie.org/), e.g., ```julia -using GLMakie # Preferred when running locally +using GLMakie # Preferred ``` or ```julia using CairoMakie ``` !!! note "Backend choice" - GLMakie and WGLMakie produce much nicer-looking animations and is also significantly faster than CairoMakie. CairoMakie may be used to produce the graphics in some web environments if constraints imposed by the web environment do not allow any of the GL alternatives. CairoMakie struggles with the Z-order of drawn objects, sometimes making bodies that should have been visible hidden behind bodies that are further back in the scene. + GLMakie and WGLMakie produce much nicer-looking animations and are also significantly faster than CairoMakie. CairoMakie may be used to produce the graphics in some web environments if constraints imposed by the web environment do not allow any of the GL alternatives. CairoMakie struggles with the Z-order of drawn objects, sometimes making bodies that should have been visible hidden behind bodies that are further back in the scene. After that, the [`render`](@ref) function is the main entry point to create 3D renderings. This function has the following methods: diff --git a/ext/Render.jl b/ext/Render.jl index 35fe12f7..884a21c2 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -309,8 +309,7 @@ function render!(scene, ::typeof(World), sys, sol, t) true end -function render!(scene, ::Union{typeof(Revolute), typeof(RevolutePlanarLoopConstraint)}, sys, sol, t) - # TODO: change to cylinder +function render!(scene, T::Union{typeof(Revolute), typeof(RevolutePlanarLoopConstraint)}, sys, sol, t) r_0 = get_fun(sol, collect(sys.frame_a.r_0)) n = get_fun(sol, collect(sys.n)) color = get_color(sys, sol, :red) @@ -321,20 +320,26 @@ function render!(scene, ::Union{typeof(Revolute), typeof(RevolutePlanarLoopConst catch 0.05f0 end |> Float32 + length = try + sol(sol.t[1], idxs=sys.length) + catch + radius + end |> Float32 thing = @lift begin - # radius = sol($t, idxs=sys.radius) O = r_0($t) n_a = n($t) R_w_a = rotfun($t) n_w = R_w_a*n_a # Rotate to the world frame - p1 = Point3f(O + radius*n_w) - p2 = Point3f(O - radius*n_w) + p1 = Point3f(O + length*n_w) + p2 = Point3f(O - length*n_w) Makie.GeometryBasics.Cylinder(p1, p2, radius) end mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) true end +render!(scene, ::typeof(Universal), sys, sol, t) = false # To recurse down to rendering the revolute joints + function render!(scene, ::typeof(Spherical), sys, sol, t) vars = get_fun(sol, collect(sys.frame_a.r_0)) color = get_color(sys, sol, :yellow) diff --git a/src/joints.jl b/src/joints.jl index dfcb89f9..079b7536 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -19,10 +19,11 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotatio # Rendering options - `radius = 0.05`: Radius of the joint in animations +- `length = radius`: Length of the joint in animations - `color`: Color of the joint in animations, a vector of length 4 with values between [0, 1] providing RGBA values """ @component function Revolute(; name, phi0 = 0, w0 = 0, n = Float64[0, 0, 1], axisflange = false, - isroot = true, iscut = false, radius = 0.05, color = [0.5019608f0,0.0f0,0.5019608f0,1.0f0], state_priority = 3.0) + 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) norm(n) ≈ 1 || error("Axis of rotation must be a unit vector") end @@ -31,6 +32,7 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotatio @parameters n[1:3]=n [description = "axis of rotation"] pars = @parameters begin radius = radius, [description = "radius of the joint in animations"] + length = length, [description = "length of the joint in animations"] color[1:4] = color, [description = "color of the joint in animations (RGBA)"] end @variables tau(t)=0 [ @@ -263,16 +265,32 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin add_params(sys, pars; name) end + +""" + Universal(; name, n_a, n_b, phi_a = 0, phi_b = 0, w_a = 0, w_b = 0, a_a = 0, a_b = 0, state_priority=10) + +Joint where `frame_a` rotates around axis `n_a` which is fixed in `frame_a` and `frame_b` rotates around axis `n_b` which is fixed in `frame_b`. The two frames coincide when `revolute_a.phi=0` and `revolute_b.phi=0`. This joint has the following potential states; + +- The relative angle `phi_a = revolute_a.phi` [rad] around axis `n_a` +- the relative angle `phi_b = revolute_b.phi` [rad] around axis `n_b` +- the relative angular velocity `w_a = D(phi_a)` +- the relative angular velocity `w_b = D(phi_b)` +""" @component function Universal(; name, n_a = [1, 0, 0], n_b = [0, 1, 0], phi_a = 0, phi_b = 0, + state_priority = 10, w_a = 0, w_b = 0, a_a = 0, - a_b = 0) + a_b = 0, + radius = 0.05f0, + length = radius, + color = [1,0,0,1] +) @named begin ptf = PartialTwoFrames() - revolute_a = Revolute(n = n_a, isroot = false) - revolute_b = Revolute(n = n_b, isroot = false) + revolute_a = Revolute(n = n_a, isroot = false, radius, length, color) + revolute_b = Revolute(n = n_b, isroot = false, radius, length, color) end @unpack frame_a, frame_b = ptf @parameters begin @@ -288,32 +306,32 @@ end @variables begin (phi_a(t) = phi_a), [ - state_priority = 10, + state_priority = state_priority, description = "Relative rotation angle from frame_a to intermediate frame", ] (phi_b(t) = phi_b), [ - state_priority = 10, + state_priority = state_priority, description = "Relative rotation angle from intermediate frame to frame_b", ] (w_a(t) = w_a), [ - state_priority = 10, + state_priority = state_priority, description = "First derivative of angle phi_a (relative angular velocity a)", ] (w_b(t) = w_b), [ - state_priority = 10, + state_priority = state_priority, description = "First derivative of angle phi_b (relative angular velocity b)", ] (a_a(t) = a_a), [ - state_priority = 10, + state_priority = state_priority, description = "Second derivative of angle phi_a (relative angular acceleration a)", ] (a_b(t) = a_b), [ - state_priority = 10, + state_priority = state_priority, description = "Second derivative of angle phi_b (relative angular acceleration b)", ] end @@ -855,12 +873,12 @@ LinearAlgebra.normalize(a::Vector{Num}) = a / norm(a) """ Planar(; n = [0,0,1], n_x = [1,0,0], cylinderlength = 0.1, cylinderdiameter = 0.05, cylindercolor = [1, 0, 1, 1], boxwidth = 0.3*cylinderdiameter, boxheight = boxwidth, boxcolor = [0, 0, 1, 1]) -Joint where frame_b can move in a plane and can rotate around an +Joint where `frame_b` can move in a plane and can rotate around an axis orthogonal to the plane. The plane is defined by vector `n` which is perpendicular to the plane and by vector `n_x`, which points in the direction of the x-axis of the plane. -frame_a and frame_b coincide when s_x=prismatic_x.s=0, -s_y=prismatic_y.s=0 and phi=revolute.phi=0. +`frame_a` and `frame_b` coincide when `s_x=prismatic_x.s=0, +s_y=prismatic_y.s=0` and `phi=revolute.phi=0`. """ @mtkmodel Planar begin @structural_parameters begin diff --git a/test/runtests.jl b/test/runtests.jl index 30af427b..ff319ad4 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -545,17 +545,13 @@ ssys = structural_simplify(IRSystem(model))#, alias_eliminate = true) # ssys = structural_simplify(model, allow_parameters = false) prob = ODEProblem(ssys, [collect(body.body.w_a .=> 0); - collect(body.body.v_0 .=> 0); - collect(D.(body.body.phi)) .=> 1; - # collect((body.body.r_0)) .=> collect((body.r_0)); - collect(D.(D.(body.body.phi))) .=> 1], (0, 10)) + collect(body.body.v_0 .=> 0);], (0, 10)) # @test_skip begin # The modelica example uses angles_fixed = true, which causes the body component to run special code for variable initialization. This is not yet supported by MTK # Without proper initialization, the example fails most of the time. Random perturbation of u0 can make it work sometimes. sol = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol) -@info "Initialization broken, initial value for body.r_0 not respected, add tests when MTK has a working initialization" doplot() && plot(sol, idxs = [body.r_0...]) |> display # end @@ -650,29 +646,28 @@ end # ============================================================================== ## universal pendulum # ============================================================================== - +using LinearAlgebra @testset "Universal pendulum" begin t = Multibody.t D = Differential(t) world = Multibody.world @named begin - joint = Universal() + joint = Universal(length=0.1) bar = FixedTranslation(r = [0, -1, 0]) - body = Body(; m = 1, isroot = false) + body = Body(; m = 1, isroot = false, r_cm=[0.1, 0, 0]) end connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, bar.frame_a) connect(bar.frame_b, body.frame_a)] @named model = ODESystem(connections, t, systems = [world, joint, bar, body]) +model = complete(model) ssys = structural_simplify(IRSystem(model)) prob = ODEProblem(ssys, [ joint.phi_b => sqrt(2); joint.revolute_a.phi => sqrt(2); - D(joint.revolute_a.phi) => 0; joint.revolute_b.phi => 0; - D(joint.revolute_b.phi) => 0 ], (0, 10)) sol2 = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol2)