make redering object oriented

docs/src/examples/free_motion.md

@@ -22,6 +22,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a)
                          systems = [world;
                                     freeMotion;
                                     body])
+model = complete(model)
 ssys = structural_simplify(IRSystem(model))
 
 prob = ODEProblem(ssys, [], (0, 10))

docs/src/examples/pendulum.md

@@ -74,7 +74,7 @@ nothing # hide
 
 ![animation](pendulum.gif)
 
-By default, the world frame is indicated using the convention x: red, y: green, z: blue. The animation shows how the simple [`Body`](@ref) represents a point mass with inertial properties at a particular distance `r_cm` away from its mounting flange `frame_a`. The cylinder that is shown connecting the pivot point to the body is for visualization purposes only, it does not have any inertial properties. To model a more physically motivated pendulum rod, we could have used a [`BodyShape`](@ref) component, which has two mounting flanges instead of one. The [`BodyShape`](@ref) component is shown in several of the examples available in the example sections of the documentation.
+By default, the world frame is indicated using the convention x: red, y: green, z: blue. The animation shows how the simple [`Body`](@ref) represents a point mass with inertial properties at a particular distance `r_cm` away from its mounting flange `frame_a`. The cylinder that is shown connecting the pivot point to the body is for visualization purposes only, it does not have any inertial properties and does not represent any physical object (hence its transparent default appearance). To model a more physically motivated pendulum rod, we could have used a [`BodyShape`](@ref) component, which has two mounting flanges instead of one. The [`BodyShape`](@ref) component is shown in several of the examples available in the example sections of the documentation.
 
 ## Adding damping
 To add damping to the pendulum such that the pendulum will eventually come to rest, we add a [`Damper`](@ref) to the revolute joint. The damping coefficient is given by `d`, and the damping force is proportional to the angular velocity of the joint. To add the damper to the revolute joint, we must create the joint with the keyword argument `axisflange = true`, this adds two internal flanges to the joint to which you can attach components from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module (1-dimensional components). We then connect one of the flanges of the damper to the axis flange of the joint, and the other damper flange to the support flange which is rigidly attached to the world.

ext/Render.jl

@@ -2,6 +2,7 @@ module Render
 using Makie
 using Multibody
 import Multibody: render, render!, loop_render, encode, decode, get_rot, get_trans, get_frame
+using Multibody.Visualizers
 using Rotations
 using LinearAlgebra
 using ModelingToolkit
@@ -240,6 +241,7 @@ render!(scene, ::Any, args...) = false # Fallback for systems that have no rende
 
 function render!(scene, ::typeof(Body), sys, sol, t)
     color = get_color(sys, sol, :purple)
+    cylinder_color = Makie.RGBA(sol(sol.t[1], idxs=sys.cylinder_color)...)
     r_cm = get_fun(sol, collect(sys.r_cm))
     framefun = get_frame_fun(sol, sys.frame_a)
     radius = sol(sol.t[1], idxs=sys.radius) |> Float32
@@ -273,38 +275,31 @@ function render!(scene, ::typeof(Body), sys, sol, t)
 
         Makie.GeometryBasics.Cylinder(origin, extremity, cylinder_radius)
     end
-    mesh!(scene, thing2; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
+    mesh!(scene, thing2; color=cylinder_color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
     true
 end
 
-function render!(scene, ::typeof(World), sys, sol, t)
+function render!(scene, ::typeof(Visualizers.CylinderPrimitive), sys, sol, t)
     sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
-    radius = 0.01f0
-    r_0 = get_fun(sol, collect(sys.frame_b.r_0))
+    radius = sol(sol.t[1], idxs=sys.radius) |> Float32
+    r_0 = get_fun(sol, collect(sys.frame_a.r_0))
+    color = get_color(sys, sol, :red)
+    r = get_fun(sol, collect(sys.r))
 
     thing = @lift begin
         O = Point3f(r_0($t)) # Assume world is never moving
-        x = O .+ Point3f(1,0,0)
-        Makie.GeometryBasics.Cylinder(O, x, radius)
-    end
-    mesh!(scene, thing, color=:red)
-
-    thing = @lift begin
-        O = Point3f(r_0($t))
-        y = O .+ Point3f(0,1,0)
-        Makie.GeometryBasics.Cylinder(O, y, radius)
+        tip = O .+ Point3f(r($t)...)
+        Makie.GeometryBasics.Cylinder(O, tip, radius)
     end
-    mesh!(scene, thing, color=:green)
+    mesh!(scene, thing; color)
 
-    thing = @lift begin
-        O = Point3f(r_0($t))
-        z = O .+ Point3f(0,0,1)
-        Makie.GeometryBasics.Cylinder(O, z, radius)
-    end
-    mesh!(scene, thing, color=:blue)
     true
 end
 
+function render!(scene, ::typeof(World), sys, sol, t)
+    false # Handled by internal viz
+end
+
 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))

src/Multibody.jl

@@ -148,6 +148,10 @@ include("orientation.jl")
 export Frame
 include("frames.jl")
 
+export Visualizers
+include("visualizers.jl")
+import .Visualizers as Viz
+
 export PartialTwoFrames
 include("interfaces.jl")
 
@@ -170,5 +174,4 @@ include("robot/robot_components.jl")
 include("robot/FullRobot.jl")
 
 
-
 end

src/components.jl

@@ -38,23 +38,27 @@ end
 """
     World(; name, render=true)
 """
-@component function World(; name, render=true, point_gravity=false)
+@component function World(; name, render=true, point_gravity=false, kwargs...)
     # World should have
     # 3+3+9+3 // r_0+f+R.R+τ
     # - (3+3) // (f+t)
     # = 12 equations 
-    @named frame_b = Frame()
     @parameters n[1:3]=[0, -1, 0] [description = "gravity direction of world"]
     @parameters g=9.80665 [description = "gravitational acceleration of world"]
     @parameters mu=3.986004418e14 [description = "Gravity field constant [m³/s²] (default = field constant of earth)"]
     @parameters render=render
     @parameters point_gravity = point_gravity
+    systems = @named begin
+        frame_b = Frame()
+        viz = Visualizers.FixedFrame(; render, kwargs...)
+    end
     O = ori(frame_b)
     eqs = Equation[collect(frame_b.r_0) .~ 0;
                    O ~ nullrotation()
                    # vec(D(O).R .~ 0); # QUESTION: not sure if I should have to add this, should only have 12 equations according to modelica paper
+                   connect(frame_b, viz.frame_a)
                    ]
-    ODESystem(eqs, t, [], [n; g; mu; point_gravity; render]; name, systems = [frame_b])
+    ODESystem(eqs, t, [], [n; g; mu; point_gravity; render]; name, systems)
 end
 
 """
@@ -221,7 +225,9 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
 # Rendering options
 - `radius`: Radius of the joint in animations
 - `cylinder_radius`: Radius of the cylinder from frame to COM in animations (only drawn if `r_cm` is non-zero). Defaults to `radius/2`
+- `length_fraction`: Fraction of the length of the body that is the cylinder from frame to COM in animations
 - `color`: Color of the joint in animations, a vector of length 4 with values between [0, 1] providing RGBA values
+- `cylinder_color`: Color of the cylinder from frame to COM in animations. Defaults to the same color as the body, but with an alpha value of 0.4
 """
 @component function Body(; name, m = 1, r_cm = [0, 0, 0],
               I_11 = 0.001,
@@ -243,6 +249,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
               length_fraction = 1,
               air_resistance = 0.0,
               color = [1,0,0,1],
+              cylinder_color = [color[1:3]; 0.4],
               quat=false,)
     @variables r_0(t)[1:3]=r_0 [
         state_priority = 2,
@@ -276,6 +283,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
         description = "Radius of the cylinder from frame to COM in animations",
     ]
     @parameters color[1:4] = color [description = "Color of the body in animations (RGBA)"]
+    @parameters cylinder_color[1:4] = cylinder_color [description = "Color of the cylinder from frame to COM in animations (RGBA)"]
     @parameters length_fraction=length_fraction, [description = "Fraction of the length of the body that is the cylinder from frame to COM in animations"]
     # @parameters I[1:3, 1:3]=I [description="inertia tensor"]
 
@@ -352,7 +360,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
     # pars = [m;r_cm;radius;I_11;I_22;I_33;I_21;I_31;I_32;color]
 
     sys = ODESystem(eqs, t; name=:nothing, metadata = Dict(:isroot => isroot), systems = [frame_a])
-    add_params(sys, [radius; cylinder_radius; color; length_fraction]; name)
+    add_params(sys, [radius; cylinder_radius; color; length_fraction; cylinder_color]; name)
 end
 
 
@@ -654,6 +662,7 @@ end
         frame_b = Frame()
         translation = FixedTranslation(r = r)
         body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2])
+        viz = Viz.Cylinder(; color, radius=diameter/2)
     end
 
     @equations begin
@@ -666,9 +675,8 @@ end
         a_0[1] ~ D(v_0[1])
         a_0[2] ~ D(v_0[2])
         a_0[3] ~ D(v_0[3])
-        connect(frame_a, translation.frame_a)
-        connect(frame_b, translation.frame_b)
-        connect(frame_a, body.frame_a)
+        connect(frame_a, translation.frame_a, body.frame_a, viz.frame_a)
+        connect(frame_b, translation.frame_b, viz.frame_b)
     end
 end
 

src/visualizers.jl

@@ -0,0 +1,115 @@
+module Visualizers
+using ModelingToolkit
+using LinearAlgebra
+using ..Multibody
+using Multibody: _norm, _normalize, resolve1
+t = Multibody.t
+
+# export CylinderPrimitive
+# export Cylinder, Frame
+
+@component function CylinderPrimitive(;
+    name,
+    render = true,
+    color = [1,0,0,1],
+    specular_coefficient = 1,
+    radius = 0.01,
+)
+    systems = @named begin
+        frame_a = Frame()
+    end
+    pars = @parameters begin
+        render = render
+        color[1:4] = color
+        specular_coefficient = specular_coefficient
+        radius = radius
+    end
+    pars = reduce(vcat, collect.(pars))
+    vars = @variables begin
+        r(t)[1:3], [description="Vector from frame_a along the length direction of the cylinder, resolved in frame_a"]
+    end
+    vars = reduce(vcat, collect.(vars))
+    eqs = Equation[
+        frame_a.f .~ 0;
+        frame_a.tau .~ 0;
+    ]
+    ODESystem(eqs, t, vars, pars; name, systems)
+end
+
+@component function Cylinder(;
+    name,
+    render = true,
+    color = [1,0,0,1],
+    specular_coefficient = 1,
+    radius = 0.01,
+)
+    pars = @parameters begin
+        # render = render
+        # color[1:4] = color
+        # specular_coefficient = specular_coefficient
+        # radius = radius
+    end
+    systems = @named begin
+        frame_a = Frame()
+        frame_b = Frame()
+        primitive = CylinderPrimitive(; render, color, specular_coefficient, radius)
+    end
+    vars = @variables begin
+        r(t)[1:3], [description="Position vector directed from the origin of frame_a to the origin of frame_b, resolved in frame_a"]
+    end
+    vars = reduce(vcat, collect.(vars))
+    eqs = Equation[
+        collect(primitive.r) .~ collect(r);
+        connect(frame_a, primitive.frame_a);
+
+        frame_b.r_0 .~ collect(frame_a.r_0) + resolve1(ori(frame_a), r);
+        ori(frame_b) ~ ori(frame_a);
+
+        zeros(3) .~ collect(frame_a.f .+ frame_b.f);
+        zeros(3) .~ collect(frame_a.tau .+ frame_b.tau .+ cross(r, frame_b.f));
+    ]
+    ODESystem(eqs, t, vars, pars; name, systems)
+end
+
+@component function FixedFrame(;
+    name,
+    render = false,
+    color_x = [1,0,0,1],
+    color_y = [0,1,0,1],
+    color_z = [0,0,1,1],
+    length = 1,
+    specular_coefficient = 1,
+    radius = 0.01,
+    r_0 = nothing,
+)
+
+    pars = @parameters begin
+        render = render
+        # color_x[1:4] = color_x
+        # color_y[1:4] = color_y
+        # color_z[1:4] = color_z
+        length = length
+        specular_coefficient = specular_coefficient
+        radius = radius
+    end
+    pars = reduce(vcat, collect.(pars))
+    vars = @variables begin
+    end
+
+    systems = @named begin
+        frame_a = Frame()
+        arrow_x = CylinderPrimitive(; color=collect(color_x), radius, render, specular_coefficient)
+        arrow_y = CylinderPrimitive(; color=collect(color_y), radius, render, specular_coefficient)
+        arrow_z = CylinderPrimitive(; color=collect(color_z), radius, render, specular_coefficient)
+    end
+    eqs = Equation[
+        connect(frame_a, arrow_x.frame_a, arrow_y.frame_a, arrow_z.frame_a);
+        collect(arrow_x.r) .~ [length,0,0];
+        collect(arrow_y.r) .~ [0,length,0];
+        collect(arrow_z.r) .~ [0,0,length];
+    ]
+    ODESystem(eqs, t, vars, pars; name, systems)
+end
+
+
+end