Merge pull request #97 from JuliaComputing/jointrender

update Universal and Spherical rendering

Author: baggepinnen

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)

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:
 

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)

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

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)