add BodyBox component (#87)

* mention some additional joints

* add BodyBox component

* add "see also" in docstring

* some progress

something weird left in either rendering or dynamics

* update rendering

* tweak tol

Author: baggepinnen

docs/src/examples/gyroscopic_effects.md

@@ -7,7 +7,7 @@ In this example, we demonstrate how a rotating body creates a reaction torque wh
 The system consists of a pendulum suspended in a spherical joint, a joint without any rotational constraints. The tip of the pendulum is a cylinder that is rotating around a revolute joint in its center. When the pendulum swings, the rotation axis of the rotating tip is changed, this causes the entire pendulum to rotate around the axis through the pendulum rod.
 
 
-```@example spring_mass_system
+```@example GYRO
 using Multibody
 using ModelingToolkit
 using Plots
@@ -42,7 +42,7 @@ ssys = structural_simplify(IRSystem(model))
 
 prob = ODEProblem(ssys, [model.world.g => 9.80665, model.revolute.w => 10], (0, 5))
 
-sol = solve(prob, Rodas5P(), abstol=1e-7, reltol=1e-7);
+sol = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8);
 @assert SciMLBase.successful_retcode(sol)
 using Test # hide
 @test sol(5, idxs=collect(model.body2.r_0[1:3])) ≈ [-0.0357364, -0.188245, 0.02076935] atol=1e-3 # hide

docs/src/examples/spherical_pendulum.md

@@ -2,7 +2,7 @@
 
 ![animation](spherical.gif)
 
-This example models a spherical pendulum. The pivot point is modeled using a [`Spherical`](@ref) joint, the pendulum rod is modeled using a [`FixedTranslation`](@ref) and the mass is modeled using a [`Body`](@ref). In this example, we choose the joint to be the root (joints are often better root objects than bodies).
+This example models a spherical pendulum. The pivot point is modeled using a [`Spherical`](@ref) joint, this lets the pendulum rotate in three directions. The pendulum rod is modeled using a [`FixedTranslation`](@ref), a component without inertial properties, and the mass of the tip is modeled using a [`Body`](@ref). To model a rod with inertial properties, see, e.g., [`BodyShape`](@ref) or [`BodyCylinder`](@ref), In this example, we choose the joint to be the root (joints are often better root objects than bodies).
 
 
 ```@example spring_mass_system

docs/src/index.md

@@ -179,7 +179,7 @@ Pages   = ["frames.jl"]
 
 A joint restricts the number of degrees of freedom (DOF) of a body. For example, a free floating body has 6 DOF, but if it is attached to a [`Revolute`](@ref) joint, the joint restricts all but one rotational degree of freedom (a revolute joint acts like a hinge). Similarily, a [`Prismatic`](@ref) joint restricts all but one translational degree of freedom (a prismatic joint acts like a slider).
 
-A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. It thus transmits no torque.
+A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. It thus transmits no torque. A [`Planar`](@ref) joint moves in a plane, i.e., it restricts one translational DOF and two rotational DOF. A [`Universal`](@ref) joint has two rotational DOF.
 
 Some joints offer the option to add 1-dimensional components to them by providing the keyword `axisflange = true`. This allows us to add, e.g., springs, dampers, sensors, and actuators to the joint.
 

ext/Render.jl

@@ -435,6 +435,44 @@ function render!(scene, ::typeof(BodyCylinder), sys, sol, t)
     true
 end
 
+function render!(scene, ::typeof(BodyBox), sys, sol, t)
+    
+    # NOTE: This draws a solid box without the hole in the middle. Cannot figure out how to render a hollow box
+    color = get_color(sys, sol, [1, 0.2, 1, 0.9])
+    width = Float32(sol(sol.t[1], idxs=sys.width))
+    height = Float32(sol(sol.t[1], idxs=sys.height))
+    length = Float32(sol(sol.t[1], idxs=sys.render_length))
+
+    length_dir = sol(sol.t[1], idxs=collect(sys.render_length_dir))
+    width_dir = sol(sol.t[1], idxs=collect(sys.render_width_dir))
+    height_dir = normalize(cross(normalize(length_dir), normalize(width_dir)))
+    width_dir = normalize(cross(height_dir, length_dir))
+
+    Rfun = get_rot_fun(sol, sys.frame_a)
+    r_0a = get_fun(sol, collect(sys.frame_a.r_0)) # Origin is translated by r_shape
+    r_shape = sol(sol.t[1], idxs=collect(sys.render_r_shape))
+    r = sol(sol.t[1], idxs=collect(sys.render_r))
+
+    R0 = [length_dir width_dir height_dir]
+    # R0 = Multibody.from_nxy(r, width_dir).R'
+    @assert isapprox(det(R0), 1.0, atol=1e-6)
+    # NOTE: The rotation by this R and the translation with r_shape needs to be double checked
+
+    origin = Vec3f(0, -width/2, -height/2) + r_shape
+    extent = Vec3f(length, width, height) 
+    thing = Makie.Rect3f(origin, extent)
+    m = mesh!(scene, thing; color, specular = Vec3f(1.5))
+    on(t) do t
+        r1 = Point3f(r_0a(t))
+        R = Rfun(t)
+        q = Rotations.QuatRotation(R*R0).q
+        Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s)
+        Makie.transform!(m, translation=r1, rotation=Q)
+    end
+
+    true
+end
+
 function render!(scene, ::typeof(Damper), sys, sol, t)
     r_0a = get_fun(sol, collect(sys.frame_a.r_0))
     r_0b = get_fun(sol, collect(sys.frame_b.r_0))

src/Multibody.jl

@@ -26,6 +26,7 @@ Create a 3D animation of a multibody system
 - `loop`: The animation will be looped this many times. Please note: looping the animation using this argument is only recommended when `display = true` for camera manipulation purposes. When the camera is not manipulated, looping the animation by other means is recommended to avoid an increase in the file size.
 - `filename` controls the name and the file type of the resulting animation
 - `traces`: An optional array of frames to show the trace of.
+- `show_axis = false`: Whether or not to show the plot axes, including background grid.
 
 # Camera control
 The following keyword arguments are available to control the camera pose:
@@ -150,7 +151,7 @@ include("frames.jl")
 export PartialTwoFrames
 include("interfaces.jl")
 
-export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, Rope
+export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope
 include("components.jl")
 
 export Revolute, Prismatic, Planar, Spherical, Universal, GearConstraint, RollingWheelJoint,

src/components.jl

@@ -357,6 +357,8 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames
 - `r`: Vector from `frame_a` to `frame_b` resolved in `frame_a`
 - All `kwargs` are passed to the internal `Body` component.
 - `shapefile`: A path::String to a CAD model that can be imported by MeshIO for 3D rendering. If none is provided, a cylinder shape is rendered.
+
+See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with predefined shapes and automatically computed inertial properties based on geometry and density.
 """
 @component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=purple, shapefile="", kwargs...)
     systems = @named begin
@@ -643,8 +645,8 @@ end
     @components begin
         frame_a = Frame()
         frame_b = Frame()
-        frameTranslation = 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], isroot, quat)
+        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])
     end
 
     @equations begin
@@ -657,8 +659,240 @@ 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, frameTranslation.frame_a)
-        connect(frame_b, frameTranslation.frame_b)
+        connect(frame_a, translation.frame_a)
+        connect(frame_b, translation.frame_b)
         connect(frame_a, body.frame_a)
     end
-end
+end
+
+
+@mtkmodel BodyBox begin
+    @structural_parameters begin
+        r = [1, 0, 0]
+        r_shape = [0, 0, 0]
+        width_dir = [0,1,0] # https://github.com/SciML/ModelingToolkit.jl/issues/2810
+        length_dir = _normalize(r - r_shape)
+        length = _norm(r - r_shape)
+    end
+    begin
+        iszero(r_shape) || error("non-zero r_shape not supported")
+        width_dir = collect(width_dir)
+        length_dir = collect(length_dir)
+    end
+
+    @parameters begin
+        # r[1:3]=r, [ # MTKs symbolic language is too weak to handle this as a symbolic parameter in from_nxy
+        #     description = "Vector from frame_a to frame_b resolved in frame_a",
+        # ]
+        # r_shape[1:3]=zeros(3), [
+        #     description = "Vector from frame_a to box origin, resolved in frame_a",
+        # ]
+        # length = _norm(r - r_shape), [
+        #     description = "Length of box",
+        # ]
+        # length_dir[1:3] = _norm(r - r_shape), [
+        #     description = "Vector in length direction of box, resolved in frame_a",
+        # ]
+
+        # width_dir[1:3] = width_dir0, [ 
+        #     description = "Vector in width direction of box, resolved in frame_a",
+        # ]
+
+        # NOTE: these are workarounds to allow rendering of this component. Unfortunately, MTK/JSCompiler cannot handle parameter arrays well enough to let these be actual parameters
+        render_r[1:3]=r, [description="For internal use only"]
+        render_r_shape[1:3]=r_shape, [description="For internal use only"]
+        render_length = length, [description="For internal use only"]
+        render_length_dir[1:3] = length_dir, [description="For internal use only"]
+        render_width_dir[1:3] = width_dir, [description="For internal use only"]
+
+
+        width = 0.3*length, [
+            description = "Width of box",
+        ]
+        height = width, [
+            description = "Height of box",
+        ]
+
+        inner_width = 0, [
+            description = "Width of inner box surface (0 <= inner_width <= width)",
+        ]
+        inner_height = inner_width, [
+            description = "Height of inner box surface (0 <= inner_height <= height)",
+        ]
+        density = 7700, [
+            description = "Density of cylinder (e.g., steel: 7700 .. 7900, wood : 400 .. 800)",
+        ]
+        color[1:4] = purple, [description = "Color of box in animations"]
+    end
+    begin
+        mo = density*length*width*height
+        mi = density*length*inner_width*inner_height
+        m = mo - mi
+        R = from_nxy(r, width_dir) 
+        r_cm = r_shape + _normalize(length_dir)*length/2
+        r_cm = collect(r_cm)
+
+        I11 = mo*(width^2 + height^2) - mi*(inner_width^2 + inner_height^2)
+        I22 = mo*(length^2 + height^2) - mi*(length^2 + inner_height^2)
+        I33 = mo*(length^2 + width^2) - mi*(length^2 + inner_width^2)
+        I = resolve_dyade1(R, Diagonal([I11, I22, I33] ./ 12)) 
+    end
+
+    @variables begin
+        r_0(t)[1:3]=zeros(3), [
+            state_priority = 2,
+            description = "Position vector from origin of world frame to origin of frame_a",
+        ]
+        v_0(t)[1:3]=zeros(3), [
+            state_priority = 2,
+            description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
+        ]
+        a_0(t)[1:3]=zeros(3), [
+            description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
+        ]
+    end
+
+    @components begin
+        frame_a = Frame()
+        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])
+    end
+
+    @equations begin
+        r_0[1] ~ ((frame_a.r_0)[1])
+        r_0[2] ~ ((frame_a.r_0)[2])
+        r_0[3] ~ ((frame_a.r_0)[3])
+        v_0[1] ~ D(r_0[1])
+        v_0[2] ~ D(r_0[2])
+        v_0[3] ~ D(r_0[3])
+        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)
+    end
+end
+
+# function BodyBox2(;
+#         r = [1, 0, 0],
+#         # r_shape = [0, 0, 0],
+#         width_dir = [0,1,0],
+#         # length_dir = _normalize(r - r_shape),
+#         # length = _norm(r - r_shape),
+
+#         # r,
+#         r_shape = nothing,
+#         length = nothing,
+#         length_dir = nothing,
+#         # width_dir = nothing,
+#         width = nothing,
+#         height = nothing,
+#         inner_width = nothing,
+#         inner_height = nothing,
+#         density = nothing,
+#         color = nothing,
+#         name,
+# )
+
+    
+#     # @parameters r[1:3]=something(r, [1,0,0]), [
+#     #         description = "Vector from frame_a to frame_b resolved in frame_a",
+#     #     ]
+#     r = collect(r)
+#     @parameters r_shape[1:3]=something(r_shape, zeros(3)), [
+#             description = "Vector from frame_a to box origin, resolved in frame_a",
+#         ]
+#     r_shape = collect(r_shape)
+#     @parameters length_dir[1:3] = something(length_dir, _norm(r - r_shape)), [
+#             description = "Vector in length direction of box, resolved in frame_a",
+#         ]
+#     length_dir = collect(length_dir)
+#     # @parameters width_dir[1:3] = something(width_dir, [0,1,0]), [ 
+#     #         description = "Vector in width direction of box, resolved in frame_a",
+#     #     ]
+#     width_dir = collect(width_dir)
+#     @parameters color[1:4] = something(color, purple), [
+#             description = "Color of box in animations"
+#         ]
+#     color = collect(color)
+#     pars = @parameters begin
+#         length = something(length, _norm(r - r_shape)), [
+#             description = "Length of box",
+#         ]
+#         width = something(width, 0.3*length), [
+#             description = "Width of box",
+#         ]
+#         height = something(height, width), [
+#             description = "Height of box",
+#         ]
+#         inner_width = something(inner_width, 0), [
+#             description = "Width of inner box surface (0 <= inner_width <= width)",
+#         ]
+#         inner_height = something(inner_height, inner_width), [
+#             description = "Height of inner box surface (0 <= inner_height <= height)",
+#         ]
+#         density = something(density, 7700), [
+#             description = "Density of box (e.g., steel: 7700 .. 7900, wood : 400 .. 800)",
+#         ]
+#     end
+#     pars = [
+#         pars; 
+#         # collect(r);
+#         collect(r_shape);
+#         collect(length_dir);
+#         # collect(width_dir);
+#         collect(color);
+#     ]
+#     mo = density*length*width*height
+#     mi = density*length*inner_width*inner_height
+#     m = mo - mi
+#     R = from_nxy(r, width_dir) 
+#     r_cm = r_shape + _normalize(length_dir)*length/2
+#     r_cm = collect(r_cm)
+
+#     I11 = mo*(width^2 + height^2) - mi*(inner_width^2 + inner_height^2)
+#     I22 = mo*(length^2 + height^2) - mi*(length^2 + inner_height^2)
+#     I33 = mo*(length^2 + width^2) - mi*(length^2 + inner_width^2)
+#     I = resolve_dyade1(R, Diagonal([I11, I22, I33] ./ 12)) 
+
+#     @variables begin
+#         r_0(t)[1:3]=zeros(3), [
+#             state_priority = 2,
+#             description = "Position vector from origin of world frame to origin of frame_a",
+#         ]
+#         v_0(t)[1:3]=zeros(3), [
+#             state_priority = 2,
+#             description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
+#         ]
+#         a_0(t)[1:3]=zeros(3), [
+#             description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
+#         ]
+#     end
+#     r_0, v_0, a_0 = collect.((r_0, v_0, a_0))
+#     vars = [r_0; v_0; a_0]
+
+#     systems = @named begin
+#         frame_a = Frame()
+#         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])
+#     end
+
+#     equations = [
+#         r_0[1] ~ ((frame_a.r_0)[1])
+#         r_0[2] ~ ((frame_a.r_0)[2])
+#         r_0[3] ~ ((frame_a.r_0)[3])
+#         v_0[1] ~ D(r_0[1])
+#         v_0[2] ~ D(r_0[2])
+#         v_0[3] ~ D(r_0[3])
+#         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)
+#     ]
+#     ODESystem(equations, t, vars, pars; name, systems)
+# end

src/joints.jl

@@ -241,6 +241,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
                     [connect_orientation(ori(frame_b), absolute_rotation(frame_a, Rrel); iscut)
                      zeros(3) .~ collect(frame_a.f) + resolve1(Rrel, frame_b.f)])
         else
+            # NOTE: this branch should never happen
             append!(eqs,
                     [connect_orientation(Rrel_inv, inverse_rotation(Rrel); iscut)
                      ori(frame_a) ~ absolute_rotation(frame_b, Rrel_inv)