add BodyCylinder component (#42)

* add BodyCylinder component

* add tests and render

* up manifest

* finish up rendering and tests of BodyCylinder

Manifest.toml

@@ -2,7 +2,7 @@
 
 julia_version = "1.10.4"
 manifest_format = "2.0"
-project_hash = "01a86afcbc8adb6e5ba61dc34569c070c3146575"
+project_hash = "b274e98a59af3f38814e912f8a2f3fcae61a2929"
 
 [[deps.ADTypes]]
 git-tree-sha1 = "fc02d55798c1af91123d07915a990fbb9a10d146"
@@ -296,9 +296,9 @@ uuid = "ade2ca70-3891-5945-98fb-dc099432e06a"
 
 [[deps.DiffEqBase]]
 deps = ["ArrayInterface", "ConcreteStructs", "DataStructures", "DocStringExtensions", "EnumX", "EnzymeCore", "FastBroadcast", "FastClosures", "ForwardDiff", "FunctionWrappers", "FunctionWrappersWrappers", "LinearAlgebra", "Logging", "Markdown", "MuladdMacro", "Parameters", "PreallocationTools", "PrecompileTools", "Printf", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "Setfield", "SparseArrays", "Static", "StaticArraysCore", "Statistics", "Tricks", "TruncatedStacktraces"]
-git-tree-sha1 = "37d49a1f8eedfe68b7622075ff3ebe3dd0e8f327"
+git-tree-sha1 = "2c6b7bf16fd850c551a765e313e7522ba455cbfd"
 uuid = "2b5f629d-d688-5b77-993f-72d75c75574e"
-version = "6.151.2"
+version = "6.151.4"
 
  [deps.DiffEqBase.extensions]
  DiffEqBaseCUDAExt = "CUDA"
@@ -469,9 +469,9 @@ uuid = "4e289a0a-7415-4d19-859d-a7e5c4648b56"
 version = "1.0.4"
 
 [[deps.EnzymeCore]]
-git-tree-sha1 = "a2f4588bde1588af6279729540099895f8dee843"
+git-tree-sha1 = "88bc63137eb033acc3afe1b9875717889c718c46"
 uuid = "f151be2c-9106-41f4-ab19-57ee4f262869"
-version = "0.7.4"
+version = "0.7.5"
 weakdeps = ["Adapt"]
 
  [deps.EnzymeCore.extensions]
@@ -735,11 +735,11 @@ version = "1.1.0"
 
 [[deps.JuliaSimCompiler]]
 deps = ["AbstractTrees", "ChainRules", "ConstructionBase", "DataStructures", "DiffEqCallbacks", "DocStringExtensions", "Expronicon", "ForwardDiff", "GPUCompiler", "Graphs", "IfElse", "JuliaFormatter", "JuliaSimBase", "JuliaSimCompilerRuntime", "LLVM", "Libdl", "LinearAlgebra", "ModelingToolkit", "NaNMath", "OffsetArrays", "RuntimeGeneratedFunctions", "SciMLBase", "Setfield", "SnoopPrecompile", "SparseArrays", "SparseDiffTools", "SpecialFunctions", "StaticArrays", "StrideArraysCore", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "UnPack"]
-git-tree-sha1 = "c4e20e91ed8d2f531bce11b41146b31e47566b2f"
+git-tree-sha1 = "44d42abc79e898c768b4b62330f7f897e9055b28"
 repo-rev = "master"
 repo-url = "git@github.com:JuliaComputing/JuliaSimCompiler.jl.git"
 uuid = "8391cb6b-4921-5777-4e45-fd9aab8cb88d"
-version = "0.1.11"
+version = "0.1.12"
 
  [deps.JuliaSimCompiler.extensions]
  AcausalVisualizationExt = "CairoMakie"
@@ -1011,10 +1011,10 @@ version = "1.2.0"
 uuid = "a63ad114-7e13-5084-954f-fe012c677804"
 
 [[deps.ModelingToolkit]]
-deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "DynamicQuantities", "ExprTools", "FindFirstFunctions", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MLStyle", "NaNMath", "OrderedCollections", "OrdinaryDiffEq", "PrecompileTools", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "SciMLStructures", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "URIs", "UnPack", "Unitful"]
-git-tree-sha1 = "9b88b3c34a2e39f64e69a07f4c36d1bdbaf82c5d"
+deps = ["AbstractTrees", "ArrayInterface", "Combinatorics", "Compat", "ConstructionBase", "DataStructures", "DiffEqBase", "DiffEqCallbacks", "DiffRules", "Distributed", "Distributions", "DocStringExtensions", "DomainSets", "DynamicQuantities", "ExprTools", "FindFirstFunctions", "ForwardDiff", "FunctionWrappersWrappers", "Graphs", "InteractiveUtils", "JuliaFormatter", "JumpProcesses", "LabelledArrays", "Latexify", "Libdl", "LinearAlgebra", "MLStyle", "NaNMath", "NonlinearSolve", "OrderedCollections", "PrecompileTools", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLBase", "SciMLStructures", "Serialization", "Setfield", "SimpleNonlinearSolve", "SparseArrays", "SpecialFunctions", "StaticArrays", "SymbolicIndexingInterface", "SymbolicUtils", "Symbolics", "URIs", "UnPack", "Unitful"]
+git-tree-sha1 = "8d43517ec8d9895f7d5f661167c05f54146a769d"
 uuid = "961ee093-0014-501f-94e3-6117800e7a78"
-version = "9.17.1"
+version = "9.19.0"
 
  [deps.ModelingToolkit.extensions]
  MTKBifurcationKitExt = "BifurcationKit"
@@ -1026,7 +1026,7 @@ version = "9.17.1"
 
 [[deps.ModelingToolkitStandardLibrary]]
 deps = ["ChainRulesCore", "DiffEqBase", "IfElse", "LinearAlgebra", "ModelingToolkit", "Symbolics"]
-git-tree-sha1 = "e36487496e263d82e866ce5f9d3e87f44765e6ea"
+git-tree-sha1 = "8cfb89cdb4f147810a45efbcc5f98ef6596d60d4"
 repo-rev = "main"
 repo-url = "https://github.com/SciML/ModelingToolkitStandardLibrary.jl.git"
 uuid = "16a59e39-deab-5bd0-87e4-056b12336739"
@@ -1043,9 +1043,9 @@ version = "0.2.4"
 
 [[deps.MultivariatePolynomials]]
 deps = ["ChainRulesCore", "DataStructures", "LinearAlgebra", "MutableArithmetics"]
-git-tree-sha1 = "dad7be0c92b688bf8f24af170825ccedc104b116"
+git-tree-sha1 = "5c1d1d9361e1417e5a065e1f84dc3686cbdaea21"
 uuid = "102ac46a-7ee4-5c85-9060-abc95bfdeaa3"
-version = "0.5.5"
+version = "0.5.6"
 
 [[deps.MutableArithmetics]]
 deps = ["LinearAlgebra", "SparseArrays", "Test"]
@@ -1071,9 +1071,9 @@ version = "1.2.0"
 
 [[deps.NonlinearSolve]]
 deps = ["ADTypes", "ArrayInterface", "ConcreteStructs", "DiffEqBase", "FastBroadcast", "FastClosures", "FiniteDiff", "ForwardDiff", "LazyArrays", "LineSearches", "LinearAlgebra", "LinearSolve", "MaybeInplace", "PrecompileTools", "Preferences", "Printf", "RecursiveArrayTools", "Reexport", "SciMLBase", "SimpleNonlinearSolve", "SparseArrays", "SparseDiffTools", "StaticArraysCore", "SymbolicIndexingInterface", "TimerOutputs"]
-git-tree-sha1 = "ed5500c66b726ec9fe8c1796c0a600353246ecf8"
+git-tree-sha1 = "40325dcea1cb84a108efe05966bbb1f4b98e5eea"
 uuid = "8913a72c-1f9b-4ce2-8d82-65094dcecaec"
-version = "3.12.4"
+version = "3.13.0"
 
  [deps.NonlinearSolve.extensions]
  NonlinearSolveBandedMatricesExt = "BandedMatrices"
@@ -1133,9 +1133,9 @@ version = "1.6.3"
 
 [[deps.OrdinaryDiffEq]]
 deps = ["ADTypes", "Adapt", "ArrayInterface", "DataStructures", "DiffEqBase", "DocStringExtensions", "EnumX", "ExponentialUtilities", "FastBroadcast", "FastClosures", "FillArrays", "FiniteDiff", "ForwardDiff", "FunctionWrappersWrappers", "IfElse", "InteractiveUtils", "LineSearches", "LinearAlgebra", "LinearSolve", "Logging", "MacroTools", "MuladdMacro", "NonlinearSolve", "Polyester", "PreallocationTools", "PrecompileTools", "Preferences", "RecursiveArrayTools", "Reexport", "SciMLBase", "SciMLOperators", "SciMLStructures", "SimpleNonlinearSolve", "SimpleUnPack", "SparseArrays", "SparseDiffTools", "StaticArrayInterface", "StaticArrays", "TruncatedStacktraces"]
-git-tree-sha1 = "b857d515bacfb46ee3603a27d6d15b196cf285d0"
+git-tree-sha1 = "a6a006cbf1e563035c0e32b63234e039b599f6b2"
 uuid = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
-version = "6.82.0"
+version = "6.83.2"
 
 [[deps.PDMats]]
 deps = ["LinearAlgebra", "SparseArrays", "SuiteSparse"]
@@ -1263,9 +1263,9 @@ version = "1.3.4"
 
 [[deps.RecursiveArrayTools]]
 deps = ["Adapt", "ArrayInterface", "DocStringExtensions", "GPUArraysCore", "IteratorInterfaceExtensions", "LinearAlgebra", "RecipesBase", "SparseArrays", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"]
-git-tree-sha1 = "3400ce27995422fb88ffcd3af9945565aad947f0"
+git-tree-sha1 = "980aabbeac7aee70d0e452a72b0c68b5b266cc7b"
 uuid = "731186ca-8d62-57ce-b412-fbd966d074cd"
-version = "3.23.1"
+version = "3.24.0"
 
  [deps.RecursiveArrayTools.extensions]
  RecursiveArrayToolsFastBroadcastExt = "FastBroadcast"
@@ -1347,9 +1347,9 @@ version = "0.6.42"
 
 [[deps.SciMLBase]]
 deps = ["ADTypes", "Accessors", "ArrayInterface", "CommonSolve", "ConstructionBase", "Distributed", "DocStringExtensions", "EnumX", "FunctionWrappersWrappers", "IteratorInterfaceExtensions", "LinearAlgebra", "Logging", "Markdown", "PrecompileTools", "Preferences", "Printf", "RecipesBase", "RecursiveArrayTools", "Reexport", "RuntimeGeneratedFunctions", "SciMLOperators", "SciMLStructures", "StaticArraysCore", "Statistics", "SymbolicIndexingInterface", "Tables"]
-git-tree-sha1 = "03e56d028b9825e087cf8bd8a1fa44af5d270ddf"
+git-tree-sha1 = "7a6c5c8c38d2e37f45d4686c3598c20c1aebf48e"
 uuid = "0bca4576-84f4-4d90-8ffe-ffa030f20462"
-version = "2.41.2"
+version = "2.41.3"
 
  [deps.SciMLBase.extensions]
  SciMLBaseChainRulesCoreExt = "ChainRulesCore"
@@ -1595,9 +1595,9 @@ version = "7.2.1+1"
 
 [[deps.SymbolicIndexingInterface]]
 deps = ["Accessors", "ArrayInterface", "RuntimeGeneratedFunctions", "StaticArraysCore"]
-git-tree-sha1 = "a5f6f138b740c9d93d76f0feddd3092e6ef002b7"
+git-tree-sha1 = "9bda3c1e2b771ab45fcbc264a078f612ca7ef427"
 uuid = "2efcf032-c050-4f8e-a9bb-153293bab1f5"
-version = "0.3.22"
+version = "0.3.23"
 
 [[deps.SymbolicLimits]]
 deps = ["SymbolicUtils"]

ext/Render.jl

@@ -70,7 +70,11 @@ function get_color(sys, sol, default)
  try
  Makie.RGBA(sol(sol.t[1], idxs=collect(sys.color))...)
  catch
- default
+ if default isa AbstractVector
+ Makie.RGBA(default...)
+ else
+ default
+ end
  end
 end
 
@@ -339,6 +343,25 @@ function render!(scene, ::typeof(BodyShape), sys, sol, t)
 end
 
 
+function render!(scene, ::typeof(BodyCylinder), sys, sol, t)
+
+ # NOTE: This draws a solid cylinder without the hole in the middle. Cannot figure out how to render a hollow cylinder
+ color = get_color(sys, sol, [1, 0.2, 1, 0.9])
+ radius = Float32(sol(sol.t[1], idxs=sys.diameter)/2)
+ 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))
+
+ 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))
@@ -421,12 +444,11 @@ function spring_mesh(p1, p2; n_wind=6, radius=0.1f0, N=200)
 end
 
 function rot_from_line(d)
- d = d ./ norm(d)
  if d[1] == 0 && d[2] == 0
  return RotMatrix{3}(Matrix{Float32}(I, 3, 3))
  end
  d = d ./ norm(d)
- z = [0, 0, 1]
+ z = SA[0, 0, 1]
  x = cross(z, d)
  x = x ./ norm(x)
  y = cross(d, x)

src/Multibody.jl

@@ -135,7 +135,7 @@ include("frames.jl")
 export PartialTwoFrames
 include("interfaces.jl")
 
-export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, Rope
+export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, Rope
 include("components.jl")
 
 export Revolute, Prismatic, Spherical, Universal, GearConstraint, RollingWheelJoint,

src/components.jl

@@ -320,7 +320,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  end
  collect(frame_a.tau .~ I * z_a + cross(w_a, I * w_a) + cross(r_cm, frame_a.f))]
 
- # pars = [m;r_cm;radius;I_11;I_22;I_33;I_21;I_31;I_32;]
+ # 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; color]; name)
@@ -449,3 +449,178 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res
 
  ODESystem(eqs, t; name, systems = [systems; links; joints])
 end
+
+# @component function BodyCylinder(; name, m = 1, r = [0.1, 0, 0], r_0 = 0, r_shape=zeros(3), length = _norm(r - r_shape), kwargs...)
+# @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 cylinder origin, resolved in frame_a",
+# # ]
+# end
+# r, r_shape = collect.((r, r_shape))
+# @parameters begin
+# dir[1:3] = r - r_shape, [
+# description = "Vector in length direction of cylinder, resolved in frame_a",
+# ]
+# length = _norm(r - r_shape), [
+# description = "Length of cylinder",
+# ]
+# length2 = _norm(r - r_shape), [ # NOTE: strange bug in JSCompiler when both I and r_cm that are parameters if Body depend on the same paramter length. Introducing a dummy parameter with the same value works around the issue. This is not ideal though, since the two parameters must have the same value.
+# description = "Length of cylinder",
+# ]
+# diameter = 1, [#length/5, [
+# description = "Diameter of cylinder",
+# ]
+# inner_diameter = 0, [
+# description = "Inner diameter of cylinder (0 <= inner_diameter <= Diameter)",
+# ]
+# density = 7700, [
+# description = "Density of cylinder (e.g., steel: 7700 .. 7900, wood : 400 .. 800)",
+# ]
+# end
+# # @variables length2(t)
+# # @assert isequal(length, length2)
+# dir = collect(dir) #.|> ParentScope # The ParentScope is required, otherwise JSCompiler thinks that these parameters belong to Body.
+# # length = ParentScope(length)
+# # diameter = ParentScope(diameter)
+# # inner_diameter = ParentScope(inner_diameter)
+# # density = ParentScope(density)
+
+# radius = diameter/2
+# innerRadius = inner_diameter/2
+# mo = density*pi*length*radius^2
+# mi = density*pi*length*innerRadius^2
+# I22 = (mo*(length^2 + 3*radius^2) - mi*(length^2 + 3*innerRadius^2))/12
+# m = mo - mi
+# R = from_nxy(r, [0, 1, 0]) 
+# r_cm = r_shape + _normalize(dir)*length2/2
+# I = resolve_dyade1(R, Diagonal([(mo*radius^2 - mi*innerRadius^2)/2, I22, I22])) 
+
+# # r_cm = ParentScope.(r_cm)
+# # I = ParentScope.(I)
+# # m = ParentScope(m)
+
+# @variables begin
+# r_0(t)[1:3]=r_0, [
+# state_priority = 2,
+# description = "Position vector from origin of world frame to origin of frame_a",
+# ]
+# v_0(t)[1:3]=0, [
+# state_priority = 2,
+# description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
+# ]
+# a_0(t)[1:3]=0, [
+# description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
+# ]
+# end
+
+# systems = @named 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], kwargs...)
+
+# end
+# r_0, v_0, a_0 = collect.((r_0, v_0, a_0))
+
+# eqs = [r_0 .~ collect(frame_a.r_0)
+# v_0 .~ D.(r_0)
+# a_0 .~ D.(v_0)
+# connect(frame_a, frameTranslation.frame_a)
+# connect(frame_b, frameTranslation.frame_b)
+# connect(frame_a, body.frame_a)]
+
+# # pars = [
+# # dir; length; diameter; inner_diameter; density
+# # ] 
+# # vars = [r_0; v_0; a_0]
+# # ODESystem(eqs, t, vars, pars; name, systems)
+# ODESystem(eqs, t; name, systems)
+# end
+
+
+@mtkmodel BodyCylinder begin
+
+ @structural_parameters begin
+ r = [1, 0, 0]
+ r_shape = [0, 0, 0]
+ 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 cylinder origin, resolved in frame_a",
+ # ]
+ dir[1:3] = r - r_shape, [
+ description = "Vector in length direction of cylinder, resolved in frame_a",
+ ]
+ length = _norm(r - r_shape), [
+ description = "Length of cylinder",
+ ]
+ length2 = _norm(r - r_shape), [ # NOTE: strange bug in JSCompiler when both I and r_cm that are parameters of Body depend on the same paramter length. Introducing a dummy parameter with the same value works around the issue. This is not ideal though, since the two parameters must have the same value.
+ description = "Length of cylinder",
+ ]
+ diameter = 1, [#length/5, [
+ description = "Diameter of cylinder",
+ ]
+ inner_diameter = 0, [
+ description = "Inner diameter of cylinder (0 <= inner_diameter <= diameter)",
+ ]
+ density = 7700, [
+ description = "Density of cylinder (e.g., steel: 7700 .. 7900, wood : 400 .. 800)",
+ ]
+ end
+ begin
+ radius = diameter/2
+ innerRadius = inner_diameter/2
+ mo = density*pi*length*radius^2
+ mi = density*pi*length*innerRadius^2
+ I22 = (mo*(length^2 + 3*radius^2) - mi*(length^2 + 3*innerRadius^2))/12
+ m = mo - mi
+ R = from_nxy(r, [0, 1, 0]) 
+ r_cm = r_shape + _normalize(dir)*length2/2
+ I = resolve_dyade1(R, Diagonal([(mo*radius^2 - mi*innerRadius^2)/2, I22, I22])) 
+ end
+
+ @variables begin
+ r_0(t)[1:3]=0, [
+ state_priority = 2,
+ description = "Position vector from origin of world frame to origin of frame_a",
+ ]
+ v_0(t)[1:3]=0, [
+ state_priority = 2,
+ description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
+ ]
+ a_0(t)[1:3]=0, [
+ description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
+ ]
+ end
+ begin
+ r_cm = collect(r_cm)
+ 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])
+ 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, frameTranslation.frame_a)
+ connect(frame_b, frameTranslation.frame_b)
+ connect(frame_a, body.frame_a)
+ end
+end