From 66b5aca1eb429bcc6a05278553c2edaf6951a02e Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 10 Jul 2024 17:35:55 +0200 Subject: [PATCH 1/2] make COM cylinder transparent by default --- docs/src/examples/pendulum.md | 2 +- ext/Render.jl | 3 ++- src/components.jl | 6 +++++- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 905106d0..296acfb9 100644 --- a/docs/src/examples/pendulum.md +++ b/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. diff --git a/ext/Render.jl b/ext/Render.jl index 286fba29..a34d4b13 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -240,6 +240,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,7 +274,7 @@ 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 diff --git a/src/components.jl b/src/components.jl index ff0cfeed..1f16b5a5 100644 --- a/src/components.jl +++ b/src/components.jl @@ -221,7 +221,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 +245,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 +279,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 +356,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 From 9f0d233bc3fddd3d7b427965eee69fe109052107 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 15 Jul 2024 12:52:13 +0200 Subject: [PATCH 2/2] make redering object oriented --- docs/src/examples/free_motion.md | 1 + ext/Render.jl | 32 ++++----- src/Multibody.jl | 5 +- src/components.jl | 16 +++-- src/visualizers.jl | 115 +++++++++++++++++++++++++++++++ 5 files changed, 143 insertions(+), 26 deletions(-) create mode 100644 src/visualizers.jl diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index fb762407..09f493a3 100644 --- a/docs/src/examples/free_motion.md +++ b/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)) diff --git a/ext/Render.jl b/ext/Render.jl index a34d4b13..3c6117cb 100644 --- a/ext/Render.jl +++ b/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 @@ -278,34 +279,27 @@ function render!(scene, ::typeof(Body), sys, sol, t) 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)) diff --git a/src/Multibody.jl b/src/Multibody.jl index f6427158..d9b04805 100644 --- a/src/Multibody.jl +++ b/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 diff --git a/src/components.jl b/src/components.jl index 1f16b5a5..62b3a043 100644 --- a/src/components.jl +++ b/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 """ @@ -658,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 @@ -670,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 diff --git a/src/visualizers.jl b/src/visualizers.jl new file mode 100644 index 00000000..fbbf0219 --- /dev/null +++ b/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 \ No newline at end of file