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/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..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 @@ -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)) 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 ff0cfeed..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 """ @@ -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 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