diff --git a/Manifest.toml b/Manifest.toml index 2e16651e..17c948c8 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -2,7 +2,7 @@ julia_version = "1.10.4" manifest_format = "2.0" -project_hash = "efeef262c86b4eafbecc9a61dea3d204d89a22d1" +project_hash = "73eed5db0c7b181f606e63adf5124204d72a3d45" [[deps.ADTypes]] git-tree-sha1 = "fc02d55798c1af91123d07915a990fbb9a10d146" @@ -166,6 +166,12 @@ git-tree-sha1 = "70232f82ffaab9dc52585e0dd043b5e0c6b714f1" uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9" version = "0.1.12" +[[deps.ColorTypes]] +deps = ["FixedPointNumbers", "Random"] +git-tree-sha1 = "b10d0b65641d57b8b4d5e234446582de5047050d" +uuid = "3da002f7-5984-5a60-b8a6-cbb66c0b333f" +version = "0.11.5" + [[deps.Combinatorics]] git-tree-sha1 = "08c8b6831dc00bfea825826be0bc8336fc369860" uuid = "861a8166-3701-5b0c-9a16-15d98fcdc6aa" @@ -451,6 +457,12 @@ version = "0.13.2" ScientificTypes = "321657f4-b219-11e9-178b-2701a2544e81" Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" +[[deps.EarCut_jll]] +deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] +git-tree-sha1 = "e3290f2d49e661fbd94046d7e3726ffcb2d41053" +uuid = "5ae413db-bbd1-5e63-b57d-d24a61df00f5" +version = "2.2.4+0" + [[deps.EnumX]] git-tree-sha1 = "bdb1942cd4c45e3c678fd11569d5cccd80976237" uuid = "4e289a0a-7415-4d19-859d-a7e5c4648b56" @@ -482,6 +494,11 @@ git-tree-sha1 = "fc3951d4d398b5515f91d7fe5d45fc31dccb3c9b" uuid = "6b7a57c9-7cc1-4fdf-b7f5-e857abae3636" version = "0.8.5" +[[deps.Extents]] +git-tree-sha1 = "94997910aca72897524d2237c41eb852153b0f65" +uuid = "411431e0-e8b7-467b-b5e0-f676ba4f2910" +version = "0.1.3" + [[deps.FastBroadcast]] deps = ["ArrayInterface", "LinearAlgebra", "Polyester", "Static", "StaticArrayInterface", "StrideArraysCore"] git-tree-sha1 = "2be93e36303143c6fffd07e2222bbade35194d9e" @@ -499,6 +516,12 @@ git-tree-sha1 = "cbf5edddb61a43669710cbc2241bc08b36d9e660" uuid = "29a986be-02c6-4525-aec4-84b980013641" version = "2.0.4" +[[deps.FileIO]] +deps = ["Pkg", "Requires", "UUIDs"] +git-tree-sha1 = "82d8afa92ecf4b52d78d869f038ebfb881267322" +uuid = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549" +version = "1.16.3" + [[deps.FileWatching]] uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee" @@ -535,6 +558,12 @@ version = "2.23.1" BlockBandedMatrices = "ffab5731-97b5-5995-9138-79e8c1846df0" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" +[[deps.FixedPointNumbers]] +deps = ["Statistics"] +git-tree-sha1 = "05882d6995ae5c12bb5f36dd2ed3f61c98cbb172" +uuid = "53c48c17-4a7d-5ca2-90c5-79b7896eea93" +version = "0.8.5" + [[deps.Format]] git-tree-sha1 = "9c68794ef81b08086aeb32eeaf33531668d5f5fc" uuid = "1fa38f19-a742-5d3f-a2b9-30dd87b9d5f8" @@ -589,6 +618,18 @@ git-tree-sha1 = "af49a0851f8113fcfae2ef5027c6d49d0acec39b" uuid = "c145ed77-6b09-5dd9-b285-bf645a82121e" version = "0.5.4" +[[deps.GeoInterface]] +deps = ["Extents"] +git-tree-sha1 = "801aef8228f7f04972e596b09d4dba481807c913" +uuid = "cf35fbd7-0cd7-5166-be24-54bfbe79505f" +version = "1.3.4" + +[[deps.GeometryBasics]] +deps = ["EarCut_jll", "Extents", "GeoInterface", "IterTools", "LinearAlgebra", "StaticArrays", "StructArrays", "Tables"] +git-tree-sha1 = "b62f2b2d76cee0d61a2ef2b3118cd2a3215d3134" +uuid = "5c1252a2-5f33-56bf-86c9-59e7332b4326" +version = "0.4.11" + [[deps.Glob]] git-tree-sha1 = "97285bbd5230dd766e9ef6749b80fc617126d496" uuid = "c27321d9-0574-5035-807b-f59d2c89b15c" @@ -658,6 +699,11 @@ git-tree-sha1 = "630b497eafcc20001bba38a4651b327dcfc491d2" uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" version = "0.2.2" +[[deps.IterTools]] +git-tree-sha1 = "42d5f897009e7ff2cf88db414a389e5ed1bdd023" +uuid = "c8e1da08-722c-5040-9ed9-7db0dc04731e" +version = "1.10.0" + [[deps.IteratorInterfaceExtensions]] git-tree-sha1 = "a3f24677c21f5bbe9d2a714f95dcd58337fb2856" uuid = "82899510-4779-5014-852e-03e436cf321d" @@ -949,6 +995,12 @@ deps = ["Artifacts", "Libdl"] uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" version = "2.28.2+1" +[[deps.MeshIO]] +deps = ["ColorTypes", "FileIO", "GeometryBasics", "Printf"] +git-tree-sha1 = "8c26ab950860dfca6767f2bbd90fdf1e8ddc678b" +uuid = "7269a6da-0436-5bbc-96c2-40638cbb6118" +version = "0.4.11" + [[deps.Missings]] deps = ["DataAPI"] git-tree-sha1 = "ec4f7fbeab05d7747bdf98eb74d130a2a2ed298d" diff --git a/Project.toml b/Project.toml index db616b9a..356831bb 100644 --- a/Project.toml +++ b/Project.toml @@ -19,10 +19,10 @@ Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" Render = ["Makie"] [compat] +CoordinateTransformations = "0.6" ModelingToolkit = "9" ModelingToolkitStandardLibrary = "2" Rotations = "1.4" -CoordinateTransformations = "0.6" julia = "1" [extras] diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 97676c8e..30ae0a04 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -199,8 +199,8 @@ W(args...; kwargs...) = Multibody.world world = W() shoulder_joint = Revolute(n = [0, 1, 0], isroot = true, axisflange = true) elbow_joint = Revolute(n = [0, 0, 1], isroot = true, axisflange = true, phi0=0.1) - upper_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0, 0.6], radius=0.05) - lower_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0.6, 0], radius=0.05) + upper_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0, 0.6], radius=0.04) + lower_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0.6, 0], radius=0.04) tip = Body(; m = 0.3, isroot = false) damper1 = RDamper(d = 0.07) diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index d25ea11b..cdd675a9 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -98,7 +98,7 @@ Multibody.jl supports automatic 3D rendering of mechanisms, we use this feature ```@example robot import CairoMakie -Multibody.render(robot, sol; filename = "robot.gif") +Multibody.render(robot, sol; y=2, lookat=[0,1,0], filename = "robot.gif") nothing # hide ``` diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index 9719609a..3b28f498 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -65,7 +65,7 @@ prob = ODEProblem(ssys, [ sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ 0.5); @test SciMLBase.successful_retcode(sol) -Multibody.render(flexible_rope, sol, filename = "flexible_rope.gif") # May take long time for n>=10 +Multibody.render(flexible_rope, sol, y = -3, x = -6, z = -6, lookat=[0, -3, 0], filename = "flexible_rope.gif") # May take long time for n>=10 ``` @@ -101,7 +101,7 @@ prob = ODEProblem(ssys, [ sol = solve(prob, Rodas4(autodiff=false)) @test SciMLBase.successful_retcode(sol) -Multibody.render(mounted_chain, sol, filename = "mounted_chain.gif") # May take long time for n>=10 +Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10 ``` ![mounted_chain animation](mounted_chain.gif) diff --git a/examples/resources/b0.stl b/examples/resources/b0.stl new file mode 100644 index 00000000..13e18fe0 Binary files /dev/null and b/examples/resources/b0.stl differ diff --git a/examples/resources/b1.stl b/examples/resources/b1.stl new file mode 100644 index 00000000..e499eaf6 Binary files /dev/null and b/examples/resources/b1.stl differ diff --git a/examples/resources/b2.stl b/examples/resources/b2.stl new file mode 100644 index 00000000..f52e138c Binary files /dev/null and b/examples/resources/b2.stl differ diff --git a/examples/resources/b3.stl b/examples/resources/b3.stl new file mode 100644 index 00000000..e275674b Binary files /dev/null and b/examples/resources/b3.stl differ diff --git a/examples/resources/b4.stl b/examples/resources/b4.stl new file mode 100644 index 00000000..540b72d0 Binary files /dev/null and b/examples/resources/b4.stl differ diff --git a/examples/resources/b5.stl b/examples/resources/b5.stl new file mode 100644 index 00000000..396f0b0e Binary files /dev/null and b/examples/resources/b5.stl differ diff --git a/examples/resources/b6.stl b/examples/resources/b6.stl new file mode 100644 index 00000000..ff729b0c Binary files /dev/null and b/examples/resources/b6.stl differ diff --git a/ext/Render.jl b/ext/Render.jl index 605499f9..6963e80c 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -1,11 +1,12 @@ module Render using Makie using Multibody -import Multibody: render, render! +import Multibody: render, render!, encode, decode using Rotations using LinearAlgebra using ModelingToolkit export render +using MeshIO, FileIO function get_rot(sol, frame, t) @@ -69,8 +70,17 @@ function get_color(sys, sol, default) end end +function get_shape(sys, sol)::String + try + sf = sol(sol.t[1], idxs=collect(sys.shapefile)) + decode(sf) + catch + "" + end +end + -function default_scene(x,y,z,lookat,up,show_axis) +function default_scene(x,y,z; lookat=Vec3f(0,0,0),up=Vec3f(0,1,0),show_axis=false) # if string(Makie.current_backend()) == "CairoMakie" # scene = Scene() # https://github.com/MakieOrg/Makie.jl/issues/3763 # fig = nothing @@ -79,7 +89,7 @@ function default_scene(x,y,z,lookat,up,show_axis) # scene = LScene(fig[1, 1], scenekw = (lights = [DirectionalLight(RGBf(1, 1, 1), Vec3f(-1, 0, 0))],)).scene # This causes a black background for CairoMakie, issue link above scene = LScene(fig[1, 1])#.scene # end - cam3d!(scene) + cam3d!(scene, center=false) # scene.scene.camera.view[] = [ # R [x,y,z]; 0 0 0 1 # ] @@ -93,9 +103,9 @@ end function render(model, sol, timevec::Union{AbstractVector, Nothing} = nothing; framerate = 30, - x = 3, - y = 0, - z = 3, + x = 2, + y = 0.5, + z = 2, lookat = Vec3f(0,0,0), up = Vec3f(0,1,0), show_axis = false, @@ -103,7 +113,7 @@ function render(model, sol, filename = "multibody_$(model.name).mp4", kwargs... ) - scene, fig = default_scene(x,y,z,lookat,up,show_axis) + scene, fig = default_scene(x,y,z; lookat,up,show_axis) if timevec === nothing timevec = range(sol.t[1], sol.t[end]*timescale, step=1/framerate) end @@ -124,7 +134,7 @@ function render(model, sol, time::Real; # fig = Figure() # scene = LScene(fig[1, 1]).scene # cam3d!(scene) - scene, fig = default_scene(0,0,10) + scene, fig = default_scene(0,0,10; kwargs...) # mesh!(scene, Rect3f(Vec3f(-5, -3.6, -5), Vec3f(10, 0.1, 10)), color=:gray) # Floor steps = range(sol.t[1], sol.t[end], length=3000) @@ -281,18 +291,36 @@ function render!(scene, ::typeof(FixedTranslation), sys, sol, t) end function render!(scene, ::typeof(BodyShape), 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)) - radius = Float32(sol(sol.t[1], idxs=sys.radius)) color = get_color(sys, sol, :purple) - thing = @lift begin - r1 = Point3f(r_0a($t)) - r2 = Point3f(r_0b($t)) - origin = r1 - extremity = r2 - Makie.GeometryBasics.Cylinder(origin, extremity, radius) + shapepath = get_shape(sys, sol) + if isempty(shapepath) + radius = Float32(sol(sol.t[1], idxs=sys.radius)) + r_0a = get_fun(sol, collect(sys.frame_a.r_0)) + r_0b = get_fun(sol, collect(sys.frame_b.r_0)) + thing = @lift begin + r1 = Point3f(r_0a($t)) + r2 = Point3f(r_0b($t)) + origin = r1 + extremity = r2 + Makie.GeometryBasics.Cylinder(origin, extremity, radius) + end + mesh!(scene, thing; color, specular = Vec3f(1.5)) + else + T = get_frame_fun(sol, sys.frame_a) + + @info "Loading shape mesh $shapepath" + shapemesh = FileIO.load(shapepath) + m = mesh!(scene, shapemesh; color, specular = Vec3f(1.5)) + + on(t) do t + Ta = T(t) + r1 = Point3f(Ta[1:3, 4]) + q = Rotations.QuatRotation(Ta[1:3, 1:3]).q + Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s) + Makie.transform!(m, translation=r1, rotation=Q) + end end - mesh!(scene, thing; color, specular = Vec3f(1.5)) + # thing = @lift begin # r1 = Point3f(sol($t, idxs=collect(sys.frame_a.r_0))) # r2 = Point3f(sol($t, idxs=collect(sys.frame_b.r_0))) diff --git a/src/Multibody.jl b/src/Multibody.jl index e1d9c216..a356b7dc 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -91,6 +91,9 @@ function at_variables_t(args...; default = nothing) xs end +encode(s) = Float64.(codeunits(s)) # Used to store strings as vectors of floats in parameters. useful for providing paths to shapefiles for 3D rendering +decode(s) = String(UInt8.(s)) + # using ModelingToolkit.SciMLBase # import SymbolicIR: InitialType # """ diff --git a/src/components.jl b/src/components.jl index 8df2998b..3961f657 100644 --- a/src/components.jl +++ b/src/components.jl @@ -343,8 +343,9 @@ 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. """ -@component function BodyShape(; name, m = 1, r = [0, 0, 0], r_cm = 0.5*r, r_0 = 0, radius = 0.08, color=purple, kwargs...) +@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 translation = FixedTranslation(r = r) body = Body(; r_cm, r_0, kwargs...) @@ -363,16 +364,19 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames @variables a_0(t)[1:3] [ guess=0, description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))", ] + + shapecode = encode(shapefile) @parameters begin r[1:3]=r, [ description = "Vector from frame_a to frame_b resolved in frame_a", ] radius = radius, [description = "Radius of the body in animations"] color[1:4] = color, [description = "Color of the body in animations"] + shapefile[1:length(shapecode)] = shapecode end - pars = [r; radius; color] + pars = [r; radius; color; shapefile] r_0, v_0, a_0 = collect.((r_0, v_0, a_0)) diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 4ba8f727..3de0ecea 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -438,6 +438,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) (tau(t)[1:6]), [guess = 0, state_priority = typemax(Int), description = "Joint driving torques"] end + path = @__DIR__() systems = @named begin axis1 = Rotational.Flange() @@ -453,6 +454,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) r5 = Revolute(n = [1, 0, 0], axisflange = true, isroot = false, radius=0.05, color=robot_orange) r6 = Revolute(n = [0, 1, 0], axisflange = true, isroot = false, radius=0.02, color=[0.5, 0.5, 0.5, 1]) b0 = BodyShape(r = [0, 0.351, 0], + shapefile = joinpath(path, "../../examples/resources/b0.stl"), # r_shape = [0, 0, 0], # lengthDirection = [1, 0, 0], # widthDirection = [0, 1, 0], @@ -460,9 +462,10 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) # width = 0.3, # height = 0.3, radius = 0.3/2, - color = robot_orange, + color = [0.5, 0.5, 0.5, 1], m = 1) b1 = BodyShape(r = [0, 0.324, 0.3], + shapefile = joinpath(path, "../../examples/resources/b1.stl"), I_22 = 1.16, # lengthDirection = [1, 0, 0], # widthDirection = [0, 1, 0], @@ -473,6 +476,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) color = robot_orange, m = 1) b2 = BodyShape(r = [0, 0.65, 0], + shapefile = joinpath(path, "../../examples/resources/b2.stl"), r_cm = [0.172, 0.205, 0], m = 56.5, I_11 = 2.58, @@ -488,6 +492,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) color = robot_orange, ) b3 = BodyShape(r = [0, 0.414, -0.155], + shapefile = joinpath(path, "../../examples/resources/b3.stl"), r_cm = [0.064, -0.034, 0], m = 26.4, I_11 = 0.279, @@ -503,6 +508,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) color = robot_orange, ) b4 = BodyShape(r = [0, 0.186, 0], + shapefile = joinpath(path, "../../examples/resources/b4.stl"), m = 28.7, I_11 = 1.67, I_22 = 0.081, @@ -516,6 +522,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) color = robot_orange, ) b5 = BodyShape(r = [0, 0.125, 0], + shapefile = joinpath(path, "../../examples/resources/b5.stl"), m = 5.2, I_11 = 1.25, I_22 = 0.81, @@ -529,6 +536,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) color = [0.5, 0.5, 0.5, 1], ) b6 = BodyShape(r = [0, 0, 0], + shapefile = joinpath(path, "../../examples/resources/b6.stl"), r_cm = [0.05, 0.05, 0.05], m = 0.5, # lengthDirection = [1, 0, 0],