add support for CAD model rendering for BodyShapes

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"

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]

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)

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
 ```
 

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)

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,17 +103,17 @@ 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,
  timescale = 1.0,
  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)))

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
 # """

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))
 

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,16 +454,18 @@ 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],
  # length = 0.225,
  # 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],