From 5b6b61633ef3bb8fa96598199c5f8773fe070495 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 8 Aug 2025 15:45:01 +0200 Subject: [PATCH 01/16] WIP rm JuliaSimCompiler --- Project.toml | 13 +++++-------- docs/src/examples/free_motion.md | 2 +- docs/src/examples/gearbox.md | 2 +- docs/src/examples/gyroscopic_effects.md | 2 +- docs/src/examples/kinematic_loops.md | 2 +- docs/src/examples/prescribed_pose.md | 2 +- docs/src/examples/quad.md | 2 +- docs/src/examples/robot.md | 2 +- docs/src/examples/ropes_and_cables.md | 2 +- docs/src/examples/sensors.md | 2 +- docs/src/examples/space.md | 2 +- docs/src/examples/spherical_pendulum.md | 2 +- docs/src/examples/spring_damper_system.md | 2 +- docs/src/examples/spring_mass_system.md | 2 +- docs/src/examples/suspension.md | 2 +- docs/src/examples/swing.md | 2 +- docs/src/examples/three_springs.md | 2 +- docs/src/examples/wheel.md | 4 ++-- src/Multibody.jl | 7 ++++++- src/PlanarMechanics/components.jl | 2 +- src/components.jl | 6 +++--- test/runtests.jl | 10 +++++----- test/test_JointUSR_RRR.jl | 2 +- test/test_PlanarMechanics.jl | 9 ++++----- test/test_fourbar.jl | 2 +- test/test_quaternions.jl | 2 +- test/test_robot.jl | 2 +- test/test_world.jl | 2 +- test/test_worldforces.jl | 2 +- 29 files changed, 48 insertions(+), 47 deletions(-) diff --git a/Project.toml b/Project.toml index aaaa97f6..f69e9e9f 100644 --- a/Project.toml +++ b/Project.toml @@ -7,7 +7,6 @@ version = "0.3.2" CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0" FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549" -JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" MeshIO = "7269a6da-0436-5bbc-96c2-40638cbb6118" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" @@ -17,13 +16,12 @@ SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" [weakdeps] -Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" +Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6" JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899" LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179" -Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6" +Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377" - [extensions] Render = ["Makie"] URDF = ["LightXML", "Graphs", "MetaGraphsNext", "JuliaFormatter"] @@ -34,7 +32,6 @@ DataInterpolations = "5, 6" FileIO = "1" Graphs = "1.0" JuliaFormatter = "1.0" -JuliaSimCompiler = "0.1.12" LightXML = "0.9" LinearAlgebra = "1.6" MeshIO = "0.4" @@ -47,12 +44,12 @@ StaticArrays = "1" julia = "1" [extras] -OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" -Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" +Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6" JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899" LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179" -Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6" MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377" +OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" +Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [targets] test = ["OrdinaryDiffEq", "Test", "JuliaFormatter", "LightXML", "Graphs", "MetaGraphsNext"] diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index 3f2ecd6b..b9085a4c 100644 --- a/docs/src/examples/free_motion.md +++ b/docs/src/examples/free_motion.md @@ -5,7 +5,7 @@ This example demonstrates how a free-floating [`Body`](@ref) can be simulated. T using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/gearbox.md b/docs/src/examples/gearbox.md index 3d1f0aeb..a34a2709 100644 --- a/docs/src/examples/gearbox.md +++ b/docs/src/examples/gearbox.md @@ -10,7 +10,7 @@ The [`GearConstraint`](@ref) has two rotational axes which do not have to be par using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/gyroscopic_effects.md b/docs/src/examples/gyroscopic_effects.md index b3b93818..d91e4b37 100644 --- a/docs/src/examples/gyroscopic_effects.md +++ b/docs/src/examples/gyroscopic_effects.md @@ -11,7 +11,7 @@ The system consists of a pendulum suspended in a spherical joint, a joint withou using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index 29f34abd..d572755c 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -29,7 +29,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational using Plots using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler t = Multibody.t D = Differential(t) diff --git a/docs/src/examples/prescribed_pose.md b/docs/src/examples/prescribed_pose.md index 2cd3f0c6..fb565dc7 100644 --- a/docs/src/examples/prescribed_pose.md +++ b/docs/src/examples/prescribed_pose.md @@ -25,7 +25,7 @@ using ModelingToolkit using Plots using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler using Test t = Multibody.t diff --git a/docs/src/examples/quad.md b/docs/src/examples/quad.md index ea416b38..7cfbea48 100644 --- a/docs/src/examples/quad.md +++ b/docs/src/examples/quad.md @@ -12,7 +12,7 @@ using ModelingToolkit using ModelingToolkitStandardLibrary.Blocks using LinearAlgebra using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq using Test t = Multibody.t diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 608f229c..33d1dd68 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -6,7 +6,7 @@ using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq using Test diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index db5a8c77..404753ae 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -15,7 +15,7 @@ We start by simulating a stiff rope that is attached to the world in one end and using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq using Test t = Multibody.t diff --git a/docs/src/examples/sensors.md b/docs/src/examples/sensors.md index 2bcb730f..3e3c083f 100644 --- a/docs/src/examples/sensors.md +++ b/docs/src/examples/sensors.md @@ -6,7 +6,7 @@ The example below adds a force and a torque sensor to the pivot point of a pendu ```@example sensor using Multibody using ModelingToolkit -using JuliaSimCompiler +# using JuliaSimCompiler using Plots using OrdinaryDiffEq using LinearAlgebra diff --git a/docs/src/examples/space.md b/docs/src/examples/space.md index 71d822da..b29ac9ce 100644 --- a/docs/src/examples/space.md +++ b/docs/src/examples/space.md @@ -18,7 +18,7 @@ In this example, we set ``\mu = 1``, `point_gravity = true` and let two masses o using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/spherical_pendulum.md b/docs/src/examples/spherical_pendulum.md index d1cbe874..4fdc4906 100644 --- a/docs/src/examples/spherical_pendulum.md +++ b/docs/src/examples/spherical_pendulum.md @@ -9,7 +9,7 @@ This example models a spherical pendulum. The pivot point is modeled using a [`S using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/spring_damper_system.md b/docs/src/examples/spring_damper_system.md index fa7375ce..e8ad18f7 100644 --- a/docs/src/examples/spring_damper_system.md +++ b/docs/src/examples/spring_damper_system.md @@ -12,7 +12,7 @@ This example has two parallel spring-mass parts, the first body (`body1`) is att using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/spring_mass_system.md b/docs/src/examples/spring_mass_system.md index b5ea518a..ac2084e9 100644 --- a/docs/src/examples/spring_mass_system.md +++ b/docs/src/examples/spring_mass_system.md @@ -12,7 +12,7 @@ This example mirrors that of the [modelica spring-mass system](https://doc.model using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica using OrdinaryDiffEq diff --git a/docs/src/examples/suspension.md b/docs/src/examples/suspension.md index 20ef81fa..6b9176b8 100644 --- a/docs/src/examples/suspension.md +++ b/docs/src/examples/suspension.md @@ -13,7 +13,7 @@ import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Transl using Plots using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler using Test t = Multibody.t diff --git a/docs/src/examples/swing.md b/docs/src/examples/swing.md index 939672a2..b6291d62 100644 --- a/docs/src/examples/swing.md +++ b/docs/src/examples/swing.md @@ -11,7 +11,7 @@ We start by defining a single rope component and attach it to a body in order to using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/three_springs.md b/docs/src/examples/three_springs.md index 31909eff..7d3072d1 100644 --- a/docs/src/examples/three_springs.md +++ b/docs/src/examples/three_springs.md @@ -11,7 +11,7 @@ The example connects three springs together in a single point. The springs are a using Multibody using ModelingToolkit using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index d43ef4ab..8c3272f9 100644 --- a/docs/src/examples/wheel.md +++ b/docs/src/examples/wheel.md @@ -26,7 +26,7 @@ import ModelingToolkitStandardLibrary.Blocks using Plots using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler using Test t = Multibody.t @@ -309,7 +309,7 @@ import Multibody.PlanarMechanics as Pl using Plots using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler using Test t = Multibody.t diff --git a/src/Multibody.jl b/src/Multibody.jl index 86c82f34..7c9ad450 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -5,7 +5,6 @@ module Multibody # foreach(println, sort(unknowns(IRSystem(model)), by=string)) using LinearAlgebra using ModelingToolkit -using JuliaSimCompiler import ModelingToolkitStandardLibrary.Mechanical.Rotational import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational import ModelingToolkitStandardLibrary.Blocks @@ -16,6 +15,12 @@ export Rotational, Translational export render, render! export subs_constants + +## JuliaSimCompiler transition helpers +export IRSystem +IRSystem(x) = x + + """ A helper function that calls `collect` on all parameters in a vector of parameters in a smart way """ diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index c91ca242..90d5683d 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -53,7 +53,7 @@ Body component with mass and inertia # Connectors: - `frame`: 2-dim. Coordinate system """ -@component function Body(; name, m, I, r = zeros(2), v=nothing, phi = nothing, w=nothing, gy = -9.80665, radius=0.1, render=true, color=Multibody.purple, state_priority=2) +@component function Body(; name, m, I, r = nothing, v=nothing, phi = nothing, w=nothing, gy = -9.80665, radius=0.1, render=true, color=Multibody.purple, state_priority=2) @named frame_a = Frame() pars = @parameters begin m = m, [description = "Mass of the body"] diff --git a/src/components.jl b/src/components.jl index bda1199f..7536ba17 100644 --- a/src/components.jl +++ b/src/components.jl @@ -377,9 +377,9 @@ This component has a single frame, `frame_a`. To represent bodies with more than neg_w = true, phi0 = zeros(3), phid0 = zeros(3), - r_0 = 0, - v_0 = 0, - w_a = 0, + r_0 = state || isroot ? 0 : nothing, + v_0 = state || isroot ? 0 : nothing, + w_a = state || isroot ? 0 : nothing, radius = 0.05, cylinder_radius = radius/2, length_fraction = 1, diff --git a/test/runtests.jl b/test/runtests.jl index 0124a47c..cfadaf59 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,7 +1,7 @@ using ModelingToolkit using Multibody using Test -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq using LinearAlgebra isdefined(Main, :t) || (t = Multibody.t) @@ -462,7 +462,7 @@ end # https://doc.modelica.org/om/Modelica.Mechanics.MultiBody.Examples.Elementary.ThreeSprings.html using Multibody using ModelingToolkit -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq @@ -602,7 +602,7 @@ sol = solve(prob, Rodas4()) using Multibody using ModelingToolkit # using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq @testset "Spherical-joint pendulum" begin @@ -686,7 +686,7 @@ end using Multibody using ModelingToolkit -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq @testset "GearConstraint" begin @@ -860,7 +860,7 @@ end ## Actuated joint using Multibody using ModelingToolkit -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq using ModelingToolkitStandardLibrary.Blocks import ModelingToolkitStandardLibrary.Mechanical.Rotational diff --git a/test/test_JointUSR_RRR.jl b/test/test_JointUSR_RRR.jl index fb6c3506..615ecb3e 100644 --- a/test/test_JointUSR_RRR.jl +++ b/test/test_JointUSR_RRR.jl @@ -5,7 +5,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational # using Plots using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler world = Multibody.world diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 29942dea..12a3e619 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -3,8 +3,9 @@ using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkit: t_nounits as t, D_nounits as D import ModelingToolkitStandardLibrary.Blocks +import Multibody: IRSystem import Multibody.PlanarMechanics as Pl -using JuliaSimCompiler +# using JuliaSimCompiler tspan = (0.0, 3.0) g = -9.80665 @@ -51,8 +52,7 @@ end ssys = structural_simplify(IRSystem(model)) @test length(unknowns(ssys)) == 2 - unset_vars = setdiff(unknowns(ssys), keys(ModelingToolkit.defaults(ssys))) - prob = ODEProblem(ssys, unset_vars .=> 0.0, tspan) + prob = ODEProblem(ssys, [ssys.body.phi => 0, ssys.body.w => 0], tspan) sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) @@ -77,8 +77,7 @@ end ssys = structural_simplify(IRSystem(model)) @test length(unknowns(ssys)) == 2 - unset_vars = setdiff(unknowns(ssys), keys(ModelingToolkit.defaults(ssys))) - prob = ODEProblem(ssys, unset_vars .=> 0.0, tspan) + prob = ODEProblem(ssys, [ssys.rod.body.phi => 0, ssys.rod.body.w => 0], tspan) sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) diff --git a/test/test_fourbar.jl b/test/test_fourbar.jl index 169e798d..c4c929ff 100644 --- a/test/test_fourbar.jl +++ b/test/test_fourbar.jl @@ -4,7 +4,7 @@ using ModelingToolkit import ModelingToolkitStandardLibrary.Mechanical.Rotational using OrdinaryDiffEq using LinearAlgebra -using JuliaSimCompiler +# using JuliaSimCompiler t = Multibody.t D = Differential(t) diff --git a/test/test_quaternions.jl b/test/test_quaternions.jl index 360db91f..e6aec0f4 100644 --- a/test/test_quaternions.jl +++ b/test/test_quaternions.jl @@ -261,7 +261,7 @@ end using Multibody using ModelingToolkit # using Plots -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq using Multibody.Rotations: params diff --git a/test/test_robot.jl b/test/test_robot.jl index fd89f180..c3e5f0cf 100644 --- a/test/test_robot.jl +++ b/test/test_robot.jl @@ -1,7 +1,7 @@ using ModelingToolkit using Multibody using Test -using JuliaSimCompiler +# using JuliaSimCompiler t = Multibody.t D = Differential(t) using OrdinaryDiffEq diff --git a/test/test_world.jl b/test/test_world.jl index 13c28231..a5f6a560 100644 --- a/test/test_world.jl +++ b/test/test_world.jl @@ -1,4 +1,4 @@ -using Multibody, ModelingToolkit, JuliaSimCompiler, Test +using Multibody, ModelingToolkit, Test, OrdinaryDiffEq @mtkmodel FallingBody begin @components begin my_world = World(g = 1, n = [0, 1, 0]) diff --git a/test/test_worldforces.jl b/test/test_worldforces.jl index d1178fb9..7d66b20e 100644 --- a/test/test_worldforces.jl +++ b/test/test_worldforces.jl @@ -1,7 +1,7 @@ using ModelingToolkit using Multibody using Test -using JuliaSimCompiler +# using JuliaSimCompiler using OrdinaryDiffEq doplot() = false world = Multibody.world From bc5249be6721d72a2548bb7761d0cb00deb78fc5 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 11 Aug 2025 09:16:50 +0200 Subject: [PATCH 02/16] lots of MTK v10 updates --- Project.toml | 4 +-- ext/Render.jl | 10 +++--- src/Multibody.jl | 4 +-- src/PlanarMechanics/utils.jl | 4 ++- src/components.jl | 29 +++++++++-------- src/fancy_joints.jl | 4 +-- src/forces.jl | 56 ++++++++++++++++---------------- src/frames.jl | 6 ++-- src/interfaces.jl | 2 +- src/joints.jl | 26 +++++++-------- test/test_orientation_getters.jl | 2 +- test/test_quaternions.jl | 15 +++++---- test/test_world.jl | 6 ++-- 13 files changed, 88 insertions(+), 80 deletions(-) diff --git a/Project.toml b/Project.toml index f69e9e9f..f609f958 100644 --- a/Project.toml +++ b/Project.toml @@ -28,7 +28,7 @@ URDF = ["LightXML", "Graphs", "MetaGraphsNext", "JuliaFormatter"] [compat] CoordinateTransformations = "0.6" -DataInterpolations = "5, 6" +DataInterpolations = "8" FileIO = "1" Graphs = "1.0" JuliaFormatter = "1.0" @@ -36,7 +36,7 @@ LightXML = "0.9" LinearAlgebra = "1.6" MeshIO = "0.4" MetaGraphsNext = "0.7" -ModelingToolkit = "9" +ModelingToolkit = "10" ModelingToolkitStandardLibrary = "2.7.2" Rotations = "1.4" SparseArrays = "1" diff --git a/ext/Render.jl b/ext/Render.jl index 53395abf..df4ff473 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -101,7 +101,7 @@ end function get_all_vars(model, sol, vars = Multibody.collect_all(unknowns(model))) @parameters render - nsrender = Multibody.ModelingToolkit.renamespace(model.name, render) + nsrender = Multibody.ModelingToolkit.renamespace(getfield(model, :name), render) dorender = try Bool(sol.prob.ps[nsrender]) catch @@ -110,10 +110,10 @@ function get_all_vars(model, sol, vars = Multibody.collect_all(unknowns(model))) for sys in model.systems if ModelingToolkit.isframe(sys) dorender || continue - newvars = Multibody.ModelingToolkit.renamespace.(model.name, Multibody.Symbolics.unwrap.(vec(ori(sys).R))) + newvars = Multibody.ModelingToolkit.renamespace.(getfield(model, :name), Multibody.Symbolics.unwrap.(vec(ori(sys).R))) append!(vars, newvars) else - subsys_ns = getproperty(model, sys.name) + subsys_ns = getproperty(model, getfield(sys, :name)) get_all_vars(subsys_ns, sol, vars) end end @@ -286,7 +286,7 @@ end function render(model, sol, timevec::Union{AbstractVector, Nothing} = nothing; - filename = "multibody_$(model.name).mp4", + filename = "multibody_$(getfield(model, :name)).mp4", framerate = default_framerate(filename), x = 2, y = 0.5, @@ -433,7 +433,7 @@ function recursive_render!(scene, model, sol, t) for subsys in model.systems system_type = get_systemtype(subsys) # did_render = render!(scene, system_type, subsys, sol, t) - subsys_ns = getproperty(model, subsys.name) + subsys_ns = getproperty(model, getfield(subsys, :name)) did_render = render!(scene, system_type, subsys_ns, sol, t) if !something(did_render, false) recursive_render!(scene, subsys_ns, sol, t) diff --git a/src/Multibody.jl b/src/Multibody.jl index 7c9ad450..b60bdb84 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -141,9 +141,9 @@ Perform validity checks on the model, such as the precense of exactly one world function multibody(model, level=0) found_world = false found_planar = false - for subsys in model.systems + for subsys in getfield(model, :systems) system_type = get_systemtype(subsys) - subsys_ns = getproperty(model, subsys.name) + subsys_ns = getproperty(model, getfield(subsys, :name)) isworld = system_type == World isplanar = system_type !== nothing && parentmodule(system_type) == PlanarMechanics found_world = found_world || isworld diff --git a/src/PlanarMechanics/utils.jl b/src/PlanarMechanics/utils.jl index 1afd923e..398a2e64 100644 --- a/src/PlanarMechanics/utils.jl +++ b/src/PlanarMechanics/utils.jl @@ -1,3 +1,5 @@ +struct IsFrame2D end + @connector function Frame(; name, render=false, length=1.0, radius=0.1) vars = @variables begin x(t), [state_priority = -1, description = "x position"] @@ -13,7 +15,7 @@ radius = radius, [description = "Radius of each axis in animations"] end - ODESystem(Equation[], t, vars, pars; name, metadata = Dict(:frame_2d => true)) + ODESystem(Equation[], t, vars, pars; name, metadata = Dict(IsFrame2D => true)) end Base.@doc """ Frame(;name) diff --git a/src/components.jl b/src/components.jl index 7536ba17..abdc23f8 100644 --- a/src/components.jl +++ b/src/components.jl @@ -2,10 +2,12 @@ using LinearAlgebra using ModelingToolkit: get_metadata import ModelingToolkitStandardLibrary +struct IsRoot end + function isroot(sys) md = get_metadata(sys) md isa Dict || return false - get(md, :isroot, false) + get(md, IsRoot, false) end purple = [0.5019608f0,0.0f0,0.5019608f0,1.0f0] @@ -21,7 +23,7 @@ If `varw = true`, the angular velocity variables `w` of the frame is also includ """ function ori(sys, varw = false) md = get_metadata(sys) - if md isa Dict && (O = get(md, :orientation, nothing)) !== nothing + if md isa AbstractDict && (O = get(md, ModelingToolkit.FrameOrientation, nothing)) !== nothing R = collect(O.R) # Since we are using this function instead of sys.ori, we need to handle namespacing properly as well ns = nameof(sys) @@ -34,7 +36,7 @@ function ori(sys, varw = false) end RotationMatrix(R, w) else - error("System $(sys.name) does not have an orientation object.") + error("System $(getfield(sys, :name)) does not have an orientation object.") end end @@ -503,7 +505,7 @@ This component has a single frame, `frame_a`. To represent bodies with more than # 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]) + sys = ODESystem(eqs, t; name=:nothing, metadata = Dict(IsRoot => isroot), systems = [frame_a]) add_params(sys, [radius; cylinder_radius; color; length_fraction; render]; name) end @@ -519,7 +521,8 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames 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="", shape_transform = I(4), shape_scale = 1, +@component function BodyShape(; name, state=false, m = 1, r = [0, 0, 0], r_cm = 0.5*r, + radius = 0.08, color=purple, shapefile="", shape_transform = I(4), shape_scale = 1, height = 0.1_norm(r), width = height, shape = "cylinder", I_11 = 0.001, I_22 = 0.001, @@ -541,14 +544,14 @@ See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with p systems = @named begin translation = FixedTranslation(r = r, render=false) translation_cm = FixedTranslation(r = r_cm, render=false) - body = Body(; m, r_cm, r_0, I_11, I_22, I_33, I_21, I_31, I_32, render=false, kwargs...) + body = Body(; m, r_cm, I_11, I_22, I_33, I_21, I_31, I_32, render=false, kwargs...) frame_a = Frame() frame_b = Frame() frame_cm = Frame() end # NOTE: these parameters should be defined before the `systems` block above, but due to bugs in MTK/JSC with higher-order array parameters we cannot do that. We still define the parameters so that they are available to make animations - @variables r_0(t)[1:3]=r_0 [ + @variables r_0(t)[1:3] [ state_priority = 2, description = "Position vector from origin of world frame to origin of frame_a", ] @@ -836,15 +839,15 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of end @variables begin - r_0(t)[1:3]=0, [ + r_0(t)[1:3], [ state_priority = 2, description = "Position vector from origin of world frame to origin of frame_a", ] - v_0(t)[1:3]=0, [ + v_0(t)[1:3], [ state_priority = 2, description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))", ] - a_0(t)[1:3]=0, [ + a_0(t)[1:3], [ description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))", ] end @@ -969,15 +972,15 @@ Rigid body with box shape. The mass properties of the body (mass, center of mass end @variables begin - r_0(t)[1:3]=zeros(3), [ + r_0(t)[1:3], [ state_priority = 2, description = "Position vector from origin of world frame to origin of frame_a", ] - v_0(t)[1:3]=zeros(3), [ + v_0(t)[1:3], [ state_priority = 2, description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))", ] - a_0(t)[1:3]=zeros(3), [ + a_0(t)[1:3], [ description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))", ] end diff --git a/src/fancy_joints.jl b/src/fancy_joints.jl index 988a5e67..2633f2b6 100644 --- a/src/fancy_joints.jl +++ b/src/fancy_joints.jl @@ -18,7 +18,7 @@ It is not possible to connect other components, such as a body with mass propert - `color`: Color of the joint in animations (RGBA) """ @component function SphericalSpherical(; name, state = false, isroot = true, iscut=false, w_rel_a_fixed = false, - r_0 = [0,0,0], + r_0 = state ? [0,0,0] : nothing, color = [1, 1, 0, 1], m = 0, radius = 0.1, @@ -461,7 +461,7 @@ end length = length, [description = "length of the joint in animations"] color[1:4] = color, [description = "color of the joint in animations (RGBA)"] end - @variables tau(t)=0 [ + @variables tau(t) [ connect = Flow, description = "Driving torque in direction of axis of rotation", ] diff --git a/src/forces.jl b/src/forces.jl index ac30aacf..c11c7498 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -12,11 +12,11 @@ function BasicTorque(; name, resolve_frame = :world) @named torque = Blocks.RealInput(; nin = 3) @unpack frame_a, frame_b = ptf @variables begin - (r_0(t)[1:3] = zeros(3)), + (r_0(t)[1:3]), [ description = "Position vector from origin of frame_a to origin of frame_b resolved in world frame", ] - (t_b_0(t)[1:3] = zeros(3)), [ + (t_b_0(t)[1:3]), [ description = "frame_b.tau resolved in world frame"] end r_0, t_b_0 = collect.((r_0, t_b_0)) @@ -45,7 +45,7 @@ function BasicTorque(; name, resolve_frame = :world) collect(frame_b.tau) .~ zeros(3)]) end - extend(ODESystem(eqs, t, name = name, systems = [torque]), ptf) + extend(System(eqs, t, name = name, systems = [torque]), ptf) end """ @@ -72,7 +72,7 @@ Torque acting between two frames, defined by 3 input signals and resolved in fra eqs = [connect(basicTorque.frame_a, frame_a) connect(basicTorque.frame_b, frame_b) connect(basicTorque.torque, torque)] - extend(ODESystem(eqs, t, name = name, systems = [torque, basicTorque]), ptf) + extend(System(eqs, t, name = name, systems = [torque, basicTorque]), ptf) end @component function BasicWorldTorque(; name, resolve_frame = :world) @@ -86,7 +86,7 @@ end collect(frame_b.tau) .~ zeros(3) end |> collect append!(eqs, collect(frame_b.f) .~ zeros(3)) - ODESystem(eqs, t; name, systems = [torque, frame_b]) + System(eqs, t; name, systems = [torque, frame_b]) end """ @@ -118,7 +118,7 @@ External torque acting at `frame_b`, defined by 3 input signals and resolved in connect(basicWorldTorque.frame_b, frame_b) connect(basicWorldTorque.torque, torque) ] - ODESystem(eqs, t, [], pars; name, systems = [torque, basicWorldTorque, frame_b]) + System(eqs, t, [], pars; name, systems = [torque, basicWorldTorque, frame_b]) end @@ -127,11 +127,11 @@ end @named force = Blocks.RealInput(; nin = 3) @unpack frame_a, frame_b = ptf @variables begin - (r_0(t)[1:3] = zeros(3)), + (r_0(t)[1:3]), [ description = "Position vector from origin of frame_a to origin of frame_b resolved in world frame", ] - (f_b_0(t)[1:3] = zeros(3)), [ + (f_b_0(t)[1:3]), [ description = "frame_b.f resolved in world frame"] end r_0, f_b_0 = collect.((r_0, f_b_0)) @@ -157,7 +157,7 @@ end error("Unknown value of argument resolve_frame") end - extend(ODESystem(eqs, t, name = name, systems = [force]), ptf) + extend(System(eqs, t, name = name, systems = [force]), ptf) end @component function BasicWorldForce(; name, resolve_frame = :world) @@ -171,7 +171,7 @@ end collect(frame_b.f) .~ zeros(3) end |> collect append!(eqs, collect(frame_b.tau) .~ zeros(3)) - ODESystem(eqs, t; name, systems = [force, frame_b]) + System(eqs, t; name, systems = [force, frame_b]) end """ @@ -198,7 +198,7 @@ Force acting between two frames, defined by 3 input signals and resolved in fram eqs = [connect(basicForce.frame_a, frame_a) connect(basicForce.frame_b, frame_b) connect(basicForce.force, force)] - extend(ODESystem(eqs, t, name = name, systems = [force, basicForce]), ptf) + extend(System(eqs, t, name = name, systems = [force, basicForce]), ptf) end """ @@ -231,11 +231,11 @@ External force acting at `frame_b`, defined by 3 input signals and resolved in f connect(basicWorldForce.frame_b, frame_b) connect(basicWorldForce.force, force) ] - ODESystem(eqs, t, [], pars; name, systems = [force, basicWorldForce, frame_b]) + System(eqs, t, [], pars; name, systems = [force, basicWorldForce, frame_b]) end @component function LineForceBase(; name, length = 0, s_small = 1e-10, fixed_rotation_at_frame_a = false, - fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0) + fixed_rotation_at_frame_b = false, r_rel_0 = nothing, s0 = nothing) @named frame_a = Frame(varw = fixed_rotation_at_frame_a) @named frame_b = Frame(varw = fixed_rotation_at_frame_b) @@ -273,7 +273,7 @@ end eqs = [eqs; frame_b.tau .~ 0] end - compose(ODESystem(eqs, t; name), frame_a, frame_b) + compose(System(eqs, t; name), frame_a, frame_b) end @component function LineForceWithMass(; name, length = 0, m = 1.0, lengthfraction = 0.5, kwargs...) @@ -283,13 +283,13 @@ end @named flange_a = TP.Flange() @named flange_b = TP.Flange() @parameters m=m [description = "mass", bounds = (0, Inf)] - @variables fa(t)=0 [description = "scalar force from flange_a"] - @variables fb(t)=0 [description = "scalar force from flange_b"] - @variables r_CM_0(t)[1:3]=zeros(3) [ + @variables fa(t) [description = "scalar force from flange_a"] + @variables fb(t) [description = "scalar force from flange_b"] + @variables r_CM_0(t)[1:3] [ description = "Position vector from world frame to point mass, resolved in world frame", ] - @variables v_CM_0(t)[1:3]=zeros(3) [description = "First derivative of r_CM_0"] - @variables ag_CM_0(t)[1:3]=zeros(3) [description = "D(v_CM_0) - gravityAcceleration"] + @variables v_CM_0(t)[1:3] [description = "First derivative of r_CM_0"] + @variables ag_CM_0(t)[1:3] [description = "D(v_CM_0) - gravityAcceleration"] @parameters lengthfraction=lengthfraction [ description = "Location of point mass with respect to frame_a as a fraction of the distance from frame_a to frame_b", bounds = (0, 1), @@ -325,7 +325,7 @@ end frame_b.f .~ -resolve2(ori(frame_b), e_rel_0 * fb)] end - extend(ODESystem(eqs, t; name, systems = [flange_a, flange_b]), lfb) + extend(System(eqs, t; name, systems = [flange_a, flange_b]), lfb) end @component function PartialLineForce(; name, kwargs...) @@ -333,15 +333,15 @@ end @unpack length, s, r_rel_0, e_rel_0, frame_a, frame_b = lfb @variables begin - (r_rel_a(t)[1:3] = 0), + (r_rel_a(t)[1:3]), [ description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a", ] - (e_a(t)[1:3] = 0), + (e_a(t)[1:3]), [ description = "Unit vector on the line connecting the origin of frame_a with the origin of frame_b resolved in frame_a (directed from frame_a to frame_b)", ] - (f(t) = 0), + (f(t)), [ description = "Line force acting on frame_a and on frame_b (positive, if acting on frame_b and directed from frame_a to frame_b)", ] @@ -355,7 +355,7 @@ end collect(frame_a.f) .~ collect(-e_a * f) collect(frame_b.f) .~ -resolve2(relative_rotation(frame_a, frame_b), frame_a.f)] - extend(ODESystem(equations, t; name), lfb) + extend(System(equations, t; name), lfb) end """ @@ -410,7 +410,7 @@ See also [`SpringDamperParallel`](@ref) @named spring2d = TP.Spring(; c, s_rel0 = s_unstretched) - @variables r_rel_a(t)[1:3]=0 [ + @variables r_rel_a(t)[1:3] [ description = "Position vector from origin of frame_a to origin of frame_b, resolved in frame_a", ] @variables e_a(t)[1:3] [ @@ -448,7 +448,7 @@ See also [`SpringDamperParallel`](@ref) connect(spring2d.flange_b, lineforce.flange_b) connect(spring2d.flange_a, lineforce.flange_a)] - sys = extend(ODESystem(eqs, t; name=:nothing, systems = [lineforce, spring2d]), ptf) + sys = extend(System(eqs, t; name=:nothing, systems = [lineforce, spring2d]), ptf) add_params(sys, pars; name) end @@ -488,7 +488,7 @@ See also [`SpringDamperParallel`](@ref) eqs = [ f ~ d * D(s), ] - extend(ODESystem(eqs, t, [s, f], pars; name), plf) + extend(System(eqs, t, [s, f], pars; name), plf) end """ @@ -525,5 +525,5 @@ where `c`, `s_unstretched` and `d` are parameters, `s` is the distance between t f ~ c * (s - s_unstretched) + f_d # lossPower ~ f_d*der(s) ] - extend(ODESystem(eqs, t, [], pars; name), plf) + extend(System(eqs, t, [], pars; name), plf) end diff --git a/src/frames.jl b/src/frames.jl index 0fca7c3a..1368581d 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -1,4 +1,4 @@ -@connector function Frame(; name, varw = false, r_0 = zeros(3), render=false, length=1.0, radius=0.1) +@connector function Frame(; name, varw = false, r_0 = nothing, render=false, length=1.0, radius=0.1) @variables r_0(t)[1:3]=r_0 [ description = "Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame", state_priority=-1, @@ -21,8 +21,8 @@ R = NumRotationMatrix(; name, varw, state_priority=-1) - ODESystem(Equation[], t, [r_0; f; tau], pars; name, - metadata = Dict(:orientation => R, :frame => true)) + ODESystem(Equation[], t, [r_0; f; tau; vec(R.R)], pars; name, + metadata = Dict(ModelingToolkit.FrameOrientation => R, ModelingToolkit.IsFrame => true)) end """ diff --git a/src/interfaces.jl b/src/interfaces.jl index 264df6fb..e5237740 100644 --- a/src/interfaces.jl +++ b/src/interfaces.jl @@ -1,5 +1,5 @@ function PartialTwoFrames(; name) @named frame_a = Frame() @named frame_b = Frame() - compose(ODESystem([], t; name), frame_a, frame_b) + compose(System(Equation[], t; name), frame_a, frame_b) end diff --git a/src/joints.jl b/src/joints.jl index a8b578b8..eeef483e 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -36,7 +36,7 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotatio color[1:4] = color, [description = "color of the joint in animations (RGBA)"] render = render, [description = "render the joint in animations"] end - @variables tau(t)=0 [ + @variables tau(t) [ connect = Flow, # state_priority = 2, description = "Driving torque in direction of axis of rotation", @@ -273,7 +273,7 @@ end """ - Universal(; name, n_a, n_b, phi_a = 0, phi_b = 0, w_a = 0, w_b = 0, a_a = 0, a_b = 0, state_priority=10) + Universal(; name, n_a, n_b, phi_a = 0, phi_b = 0, w_a = 0, w_b = 0, a_a = nothing, a_b = nothing, state_priority=10) Joint where `frame_a` rotates around axis `n_a` which is fixed in `frame_a` and `frame_b` rotates around axis `n_b` which is fixed in `frame_b`. The two frames coincide when `revolute_a.phi=0` and `revolute_b.phi=0`. This joint has the following potential states; @@ -287,8 +287,8 @@ Joint where `frame_a` rotates around axis `n_a` which is fixed in `frame_a` and state_priority = 10, w_a = 0, w_b = 0, - a_a = 0, - a_b = 0, + a_a = nothing, + a_b = nothing, radius = 0.05f0, length = radius, color = [1,0,0,1] @@ -415,13 +415,13 @@ This ideal massless joint provides a gear constraint between frames `frame_a` an state_priority = 10, description = "Relative angular velocity of revolute joint at frame_b", ] - (a_b(t) = 0), + (a_b(t)), [ state_priority = 10, description = "Relative angular acceleration of revolute joint at frame_b", ] - (totalPower(t) = 0), [description = "Total power flowing into this element"] + (totalPower(t)), [description = "Total power flowing into this element"] end eqs = [phi_b ~ actuatedRevolute_b.phi @@ -484,12 +484,12 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi iscut = false, state_priority = 4, phid = 0, - phidd = 0, + phidd = nothing, neg_w = true, w_rel_b = 0, r_rel_a = 0, v_rel_a = 0, - a_rel_a = 0) + a_rel_a = nothing) @named begin frame_a = Frame() frame_b = Frame() @@ -735,9 +735,9 @@ s_y=prismatic_y.s=0` and `phi=revolute.phi=0`. (v_x(t) = 0), [state_priority = 3.0, description = "Relative velocity along first prismatic joint"] (v_y(t) = 0), [state_priority = 3.0, description = "Relative velocity along second prismatic joint"] (w(t) = 0), [state_priority = 3.0, description = "Relative angular velocity around revolute joint"] - (a_x(t) = 0), [description = "Relative acceleration along first prismatic joint"] - (a_y(t) = 0), [description = "Relative acceleration along second prismatic joint"] - (wd(t) = 0), [description = "Relative angular acceleration around revolute joint"] + (a_x(t)), [description = "Relative acceleration along first prismatic joint"] + (a_y(t)), [description = "Relative acceleration along second prismatic joint"] + (wd(t)), [description = "Relative angular acceleration around revolute joint"] end @equations begin s_x ~ prismatic_x.s @@ -789,8 +789,8 @@ end (phi(t) = 0), [state_priority = 200, description = "Relative rotation angle from frame_a to frame_b"] (v(t) = 0), [state_priority = 200, description = "First derivative of s (relative velocity)"] (w(t) = 0), [state_priority = 200, description = "First derivative of angle phi (relative angular velocity)"] - (a(t) = 0), [description = "Second derivative of s (relative acceleration)"] - (wd(t) = 0), [description = "Second derivative of angle phi (relative angular acceleration)"] + (a(t)), [description = "Second derivative of s (relative acceleration)"] + (wd(t)), [description = "Second derivative of angle phi (relative angular acceleration)"] end @equations begin diff --git a/test/test_orientation_getters.jl b/test/test_orientation_getters.jl index 8a69deae..89ad8e65 100644 --- a/test/test_orientation_getters.jl +++ b/test/test_orientation_getters.jl @@ -33,7 +33,7 @@ model = complete(model) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.shoulder_joint.phi => 0.0, model.elbow_joint.phi => 0.1, model.world.g => 9.81], (0, 12)) -sol = solve(prob, Rodas4()) +sol = solve(prob, Rodas5P()) diff --git a/test/test_quaternions.jl b/test/test_quaternions.jl index e6aec0f4..5051e51a 100644 --- a/test/test_quaternions.jl +++ b/test/test_quaternions.jl @@ -1,7 +1,10 @@ using Test +using Multibody, ModelingToolkit, OrdinaryDiffEq import Multibody.Rotations.QuatRotation as Quat import Multibody.Rotations import Multibody.Rotations: RotXYZ +t = Multibody.t +D = Multibody.D function get_R(sol, frame, t) reshape(sol(t, idxs=vec(ori(frame).R.mat)), 3, 3) end @@ -24,7 +27,7 @@ using LinearAlgebra connections = [connect(world.frame_b, spring.frame_a) connect(spring.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, systems = [world, spring, body]) +@named model = System(connections, t, systems = [world, spring, body]) model = complete(model) # ssys = structural_simplify(model, allow_parameter = false) @@ -68,7 +71,7 @@ end # ============================================================================== ## Simple motion with quaternions=============================================== # ============================================================================== -using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler +using LinearAlgebra, ModelingToolkit, Multibody using OrdinaryDiffEq, Test @testset "Simple motion with quaternions and state in Body" begin @@ -87,7 +90,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [world, joint, body]) irsys = IRSystem(model) ssys = structural_simplify(irsys) @@ -155,7 +158,7 @@ end connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, body.frame_a)] - @named model = ODESystem(connections, t, + @named model = System(connections, t, systems = [world, joint, body]) irsys = IRSystem(model) ssys = structural_simplify(irsys) @@ -213,7 +216,7 @@ end connect(rod.frame_b, body.frame_a)] - @named model = ODESystem(connections, t, + @named model = System(connections, t, systems = [world, joint, body, rod]) irsys = IRSystem(model) ssys = structural_simplify(irsys) @@ -284,7 +287,7 @@ using Multibody.Rotations: params connect(spring1.frame_a, world.frame_b) connect(body.frame_b, spring2.frame_b)] - @named model = ODESystem(eqs, t, + @named model = System(eqs, t, systems = [ world, body, diff --git a/test/test_world.jl b/test/test_world.jl index a5f6a560..a9af28a4 100644 --- a/test/test_world.jl +++ b/test/test_world.jl @@ -10,7 +10,7 @@ end model = complete(model) ssys = structural_simplify(IRSystem(model)) prob = ODEProblem(ssys, [], (0, 1)) -sol = solve(prob, Tsit5()) +sol = solve(prob, Rodas5P()) tv = 0:0.1:1 @test iszero(sol(tv, idxs=model.body.r_0[1])) @@ -20,14 +20,14 @@ tv = 0:0.1:1 # Change g prob = ODEProblem(ssys, [model.my_world.g .=> 2], (0, 1)) -sol = solve(prob, Tsit5()) +sol = solve(prob, Rodas5P()) @test iszero(sol(tv, idxs=model.body.r_0[1])) @test sol(tv, idxs=model.body.r_0[2]) ≈ 2*tv.^2 ./ 2 atol=1e-6 @test iszero(sol(tv, idxs=model.body.r_0[3])) # Change n prob = ODEProblem(ssys, collect(model.my_world.n) .=> [1, 0, 0], (0, 1)) -sol = solve(prob, Tsit5()) +sol = solve(prob, Rodas5P()) @test sol(tv, idxs=model.body.r_0[1]) ≈ tv.^2 ./ 2 atol=1e-6 @test iszero(sol(tv, idxs=model.body.r_0[2])) @test iszero(sol(tv, idxs=model.body.r_0[3])) From 8846d499dcad27b3fb9cbb02348f5a7115f54e24 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 11 Aug 2025 09:34:31 +0200 Subject: [PATCH 03/16] more updates --- src/PlanarMechanics/components.jl | 22 +++++++-------- src/PlanarMechanics/sensors.jl | 8 +++--- src/PlanarMechanics/utils.jl | 2 +- test/test_PlanarMechanics.jl | 45 ++++++++++++++----------------- 4 files changed, 35 insertions(+), 42 deletions(-) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 90d5683d..7f639252 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -242,9 +242,9 @@ Linear 2D translational spring end @variables begin - s_relx(t) = 0 - s_rely(t) = 0 - phi_rel(t) = 0 + s_relx(t) + s_rely(t) + phi_rel(t) f_x(t) f_y(t) end @@ -296,12 +296,12 @@ Linear (velocity dependent) damper end @variables begin - r0x(t) = 0 - r0y(t) = 0 - d0x(t) = 0 - d0y(t) = 0 - vx(t) = 0 - vy(t) = 0 + r0x(t) + r0y(t) + d0x(t) + d0y(t) + vx(t) + vy(t) v(t) f(t) end @@ -384,10 +384,10 @@ Linear 2D translational spring damper model @variables begin v_relx(t) v_rely(t) - w_rel(t) = 0 + w_rel(t) s_relx(t) s_rely(t) - phi_rel(t) = 0 + phi_rel(t) f_x(t) f_y(t) tau(t) diff --git a/src/PlanarMechanics/sensors.jl b/src/PlanarMechanics/sensors.jl index 27591400..56e3d56c 100644 --- a/src/PlanarMechanics/sensors.jl +++ b/src/PlanarMechanics/sensors.jl @@ -440,8 +440,7 @@ end end @component function AbsoluteVelocity(; name, resolve_in_frame = :frame_a) - @named partial_abs_sensor = PartialAbsoluteSensor() - @unpack frame_a, = partial_abs_sensor + @named frame_a = Frame() @named v_x = RealOutput() @named v_y = RealOutput() @@ -684,8 +683,7 @@ end end @component function AbsoluteAcceleration(; name, resolve_in_frame = :frame_a) - @named partial_abs_sensor = PartialAbsoluteSensor() - @unpack frame_a, = partial_abs_sensor + @named frame_a = Frame() @named a_x = RealOutput() @named a_y = RealOutput() @@ -730,7 +728,7 @@ end zero_pos1.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return System(eqs, t; name, systems) end @component function RelativeAcceleration(; name, resolve_in_frame = :frame_a) diff --git a/src/PlanarMechanics/utils.jl b/src/PlanarMechanics/utils.jl index 398a2e64..2065ad61 100644 --- a/src/PlanarMechanics/utils.jl +++ b/src/PlanarMechanics/utils.jl @@ -15,7 +15,7 @@ struct IsFrame2D end radius = radius, [description = "Radius of each axis in animations"] end - ODESystem(Equation[], t, vars, pars; name, metadata = Dict(IsFrame2D => true)) + System(Equation[], t, vars, pars; name, metadata = Dict(IsFrame2D => true)) end Base.@doc """ Frame(;name) diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 12a3e619..082b71ad 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -14,7 +14,7 @@ g = -9.80665 m = 2 I = 1 @named body = Pl.Body(; m, I) - @named model = ODESystem(Equation[], + @named model = System(Equation[], t, [], [], @@ -45,7 +45,7 @@ end connect(rod.frame_b, body.frame_a) ] - @named model = ODESystem(connections, + @named model = System(connections, t, systems = [body, revolute, rod, ceiling]) model = complete(model) @@ -70,7 +70,7 @@ end connect(revolute.frame_b, rod.frame_a), ] - @named model = ODESystem(connections, + @named model = System(connections, t, systems = [revolute, rod, ceiling]) model = complete(model) @@ -92,12 +92,11 @@ end @testset "AbsoluteAccCentrifugal" begin m = 1 - I = 0.1 w = 10 resolve_in_frame = :world # components - @named body = Pl.Body(; m, I, gy = 0.0) + @named body = Pl.Body(; m, I=0.1, gy = 0.0) @named fixed_translation = Pl.FixedTranslation(; r = [10, 0]) @named fixed = Pl.Fixed() @named revolute = Pl.Revolute()#constant_w = w) @@ -113,7 +112,7 @@ end # connect(body.frame_a, abs_v_sensor.frame_a) ] - @named model = ODESystem(eqs, + @named model = System(eqs, t, [], [], @@ -125,7 +124,7 @@ end abs_v_sensor ]) model = complete(model) - @test_skip begin # Yingbo: BoundsError: attempt to access 137-element Vector{Vector{Int64}} at index [138] + begin # Yingbo: BoundsError: attempt to access 137-element Vector{Vector{Int64}} at index [138] ssys = structural_simplify(IRSystem(model)) prob = ODEProblem(ssys, [model.body.w => w], tspan) sol = solve(prob, Rodas5P(), initializealg=BrownFullBasicInit()) @@ -142,7 +141,7 @@ end # instantaneous linear acceleration a_signal(t) = -w^3 * cos.(w .* t) - @test all(a_signal.(test_points) .≈ sol.(test_points; idxs = body.ax)) + @test all(a_signal.(test_points) .≈ sol.(test_points; idxs = body.a[1])) end end @@ -201,7 +200,7 @@ end # connect(body2.frame_a, rel_a_sensor2.frame_b), # ] - @named model = ODESystem(connections, + @named model = System(connections, t, [], [], @@ -260,7 +259,7 @@ end end @testset "Measure Demo" begin - @named body = Pl.Body(; m = 1, I = 0.1) + @named body = Pl.Body(; m = 1, I = 0.1, phi=0) @named fixed_translation = Pl.FixedTranslation(;) @named fixed = Pl.Fixed() @named body1 = Pl.Body(; m = 0.4, I = 0.02) @@ -291,10 +290,8 @@ end Pl.connect_sensor(body1.frame_a, abs_a_sensor.frame_a)... # Pl.connect_sensor(body1.frame, abs_v_sensor.frame_a)..., # Pl.connect_sensor(body1.frame, abs_pos_sensor.frame_a)..., ] - @named model = ODESystem(connections, + @named model = System(connections, t, - [], - [], systems = [ fixed_translation, body, @@ -304,14 +301,12 @@ end revolute1, revolute2, abs_pos_sensor - ]) - @test_skip begin # Yingbo: BoundsError again - sys = structural_simplify(IRSystem(model)) - unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5)) - sol = solve(prob, Rodas5P()) - @test SciMLBase.successful_retcode(sol) - end + ]) + sys = structural_simplify(IRSystem(model)) + # unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, [], (0, 5)) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) end @testset "SpringDamper" begin @@ -333,7 +328,7 @@ end connect(fixed_translation.frame_b, spring_damper.frame_a), connect(spring_damper.frame_b, body.frame_a) ] - @named model = ODESystem(connections, + @named model = System(connections, t, [], [], @@ -344,8 +339,8 @@ end fixed_translation ]) sys = structural_simplify(IRSystem(model)) - unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) - prob = ODEProblem(sys, unset_vars .=> 0.0, (0, 5)) + # unset_vars = setdiff(unknowns(sys), keys(ModelingToolkit.defaults(sys))) + prob = ODEProblem(sys, [], (0, 5)) sol = solve(prob, Rodas5P()) @test SciMLBase.successful_retcode(sol) end @@ -366,7 +361,7 @@ end connect(prismatic.frame_b, spring.frame_b) ] - @named model = ODESystem(connections, + @named model = System(connections, t, [], [], From ae551d02ec3220ec0a7901e950ce4cbe3999adfa Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 11 Aug 2025 10:00:10 +0200 Subject: [PATCH 04/16] rm JuliaSimCompiler from more code --- docs/Project.toml | 1 - docs/src/examples/pendulum.md | 6 +++--- docs/src/examples/robot.md | 4 ++-- docs/src/index.md | 4 ++-- examples/JuliaSim_logo.jl | 2 +- ext/Project.toml | 1 - ext/URDF.jl | 2 +- src/Multibody.jl | 6 ++++-- src/fancy_joints.jl | 11 +++++++---- test/runtests.jl | 2 +- test/test_quaternions.jl | 4 ++-- test/test_urdf.jl | 2 +- 12 files changed, 24 insertions(+), 21 deletions(-) diff --git a/docs/Project.toml b/docs/Project.toml index d9bfc821..543b7d0d 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -3,7 +3,6 @@ ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a" -JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d" Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739" diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 74731187..4c048812 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -4,7 +4,7 @@ This beginners tutorial will start by modeling a pendulum pivoted around the ori To start, we load the required packages ```@example pendulum using ModelingToolkit -using Multibody, JuliaSimCompiler +using Multibody using OrdinaryDiffEq # Contains the ODE solver we will use using Plots ``` @@ -41,7 +41,7 @@ nothing # hide With all components and connections defined, we can create an `ODESystem` like so: ```@example pendulum -@named model = ODESystem(connections, t, systems=[world, joint, body]) +@named model = System(connections, t, systems=[world, joint, body]) model = complete(model) nothing # hide ``` @@ -187,7 +187,7 @@ The systems we have modeled so far have all been _planar_ mechanisms. We now ext This pendulum, sometimes referred to as a _rotary pendulum_, has two joints, one in the "shoulder", which is typically configured to rotate around the gravitational axis, and one in the "elbow", which is typically configured to rotate around the axis of the upper arm. The upper arm is attached to the shoulder joint, and the lower arm is attached to the elbow joint. The tip of the pendulum is attached to the lower arm. ```@example pendulum -using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq, Plots +using ModelingToolkit, Multibody, OrdinaryDiffEq, Plots import ModelingToolkitStandardLibrary.Mechanical.Rotational.Damper as RDamper import Multibody.Rotations diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 33d1dd68..c1d04dc9 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -109,7 +109,7 @@ The coordinates of any point on the mechanism may be obtained in the world coord ```@example robot output = collect(robot.mechanics.b6.frame_b.r_0) -fkine = JuliaSimCompiler.build_explicit_observed_function(ssys, output) +fkine = build_explicit_observed_function(ssys, output) fkine(prob.u0, prob.p, 0) ``` @@ -124,7 +124,7 @@ prob[output] or by building an explicit function `(state, parameters, time) -> output` ```@example robot -fkine = JuliaSimCompiler.build_explicit_observed_function(ssys, output) +fkine = build_explicit_observed_function(ssys, output) fkine(prob.u0, prob.p, 0) ``` !!! note diff --git a/docs/src/index.md b/docs/src/index.md index 058e44a0..92e32d1e 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -8,7 +8,7 @@ Documentation for [Multibody](https://github.com/JuliaComputing/Multibody.jl). ```@setup logo using ModelingToolkit -using Multibody, JuliaSimCompiler +using Multibody using OrdinaryDiffEq # Contains the ODE solver we will use using Plots t = Multibody.t @@ -164,7 +164,7 @@ The following animations give a quick overview of simple mechanisms that can be ## Installation To install this library, first follow the [installation instructions for JuliaSimCompiler](https://juliacomputing.github.io/JuliaSimCompiler.jl/stable/#Installing-and-Using-JuliaSimCompiler). In particular, you need to [add the JuliaHub Registry](https://help.juliahub.com/juliasim/dev/gettingstarted/juliahubregistry/). -After the registry is added and JuliaSimCompiler is installed, you may install this package using +After the registry is added, you may install this package using ```julia import Pkg Pkg.add("Multibody") diff --git a/examples/JuliaSim_logo.jl b/examples/JuliaSim_logo.jl index 4aa7dd58..57ca4c1d 100644 --- a/examples/JuliaSim_logo.jl +++ b/examples/JuliaSim_logo.jl @@ -1,5 +1,5 @@ using ModelingToolkit -using Multibody, JuliaSimCompiler +using Multibody using OrdinaryDiffEq # Contains the ODE solver we will use using Plots t = Multibody.t diff --git a/ext/Project.toml b/ext/Project.toml index bc71a546..e4aae752 100644 --- a/ext/Project.toml +++ b/ext/Project.toml @@ -2,7 +2,6 @@ CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6" JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899" -JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d" LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179" Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377" diff --git a/ext/URDF.jl b/ext/URDF.jl index a6465912..355ec2ea 100644 --- a/ext/URDF.jl +++ b/ext/URDF.jl @@ -308,7 +308,7 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no s = if extras """ - using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq, Plots + using ModelingToolkit, Multibody, OrdinaryDiffEq, Plots import ModelingToolkit: t_nounits as t, D_nounits as D """ else diff --git a/src/Multibody.jl b/src/Multibody.jl index b60bdb84..1746f181 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -99,7 +99,9 @@ If this optimization is to be performed repeatedly for several simulations of th """ function subs_constants(model, c=[0, 1]; ssys = structural_simplify(IRSystem(model)), kwargs...) inds = find_defaults_with_val(model, c; ssys, kwargs...) - ssys = JuliaSimCompiler.freeze_parameters(ssys, inds) + # ssys = JuliaSimCompiler.freeze_parameters(ssys, inds) + @error "JuliaSimCompiler.freeze_parameters is no longer available. This optimization is currently disabled." + return ssys end function find_defaults_with_val(model, c=[0, 1]; defs = defaults(model), ssys = structural_simplify(IRSystem(model))) @@ -229,7 +231,7 @@ Translate a URDF file into a Multibody model. Only available if LightXML.jl, Gra Example usage: ``` -using Multibody, ModelingToolkit, JuliaSimCompiler, LightXML, Graphs, MetaGraphsNext, JuliaFormatter +using Multibody, ModelingToolkit, LightXML, Graphs, MetaGraphsNext, JuliaFormatter urdf2multibody(joinpath(dirname(pathof(Multibody)), "..", "test/doublependulum.urdf"), extras=true, out="/tmp/urdf_import.jl") ``` diff --git a/src/fancy_joints.jl b/src/fancy_joints.jl index 2633f2b6..0a0ffe22 100644 --- a/src/fancy_joints.jl +++ b/src/fancy_joints.jl @@ -487,10 +487,13 @@ end Rrel = planar_rotation(e, angle, D(angle)) Rb = absolute_rotation(ori(frame_a), Rrel) - IR = JuliaSimCompiler - e_array = IR.make_array((3,), e...) - r_a_array = IR.make_array((3,), r_a...) - r_b_array = IR.make_array((3,), r_b...) + # IR = JuliaSimCompiler + # e_array = IR.make_array((3,), e...) + # r_a_array = IR.make_array((3,), r_a...) + # r_b_array = IR.make_array((3,), r_b...) + e_array = collect(e) + r_a_array = collect(r_a) + r_b_array = collect(r_b) eqs = [ collect(r_a) .~ collect(position_a.u) diff --git a/test/runtests.jl b/test/runtests.jl index cfadaf59..e3076ff5 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1147,7 +1147,7 @@ end ## -using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq +using LinearAlgebra, ModelingToolkit, Multibody, OrdinaryDiffEq using Multibody.Rotations: RotXYZ world = Multibody.world diff --git a/test/test_quaternions.jl b/test/test_quaternions.jl index 5051e51a..c4734626 100644 --- a/test/test_quaternions.jl +++ b/test/test_quaternions.jl @@ -148,7 +148,7 @@ end # ============================================================ @testset "Quaternions and state in free motion" begin - using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler + using LinearAlgebra, ModelingToolkit, Multibody t = Multibody.t world = Multibody.world @@ -200,7 +200,7 @@ end # ============================================================ # @testset "Spherical joint with quaternion state" begin - using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler + using LinearAlgebra, ModelingToolkit, Multibody world = Multibody.world diff --git a/test/test_urdf.jl b/test/test_urdf.jl index 71d920fd..2d5cdf56 100644 --- a/test/test_urdf.jl +++ b/test/test_urdf.jl @@ -9,7 +9,7 @@ include("doublependulum.jl") -using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq#, Plots +using ModelingToolkit, Multibody, OrdinaryDiffEq#, Plots import ModelingToolkit: t_nounits as t, D_nounits as D @named model = DoublePendulum() From 06a633063a72de8ede3c6964aca58e7f8569bf44 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 11 Aug 2025 10:02:36 +0200 Subject: [PATCH 05/16] ODESystem -> System --- docs/src/examples/free_motion.md | 4 +- docs/src/examples/gearbox.md | 2 +- docs/src/examples/gyroscopic_effects.md | 2 +- docs/src/examples/kinematic_loops.md | 6 +-- docs/src/examples/pendulum.md | 16 ++++---- docs/src/examples/quad.md | 2 +- docs/src/examples/ropes_and_cables.md | 6 +-- docs/src/examples/sensors.md | 2 +- docs/src/examples/spherical_pendulum.md | 2 +- docs/src/examples/spring_damper_system.md | 2 +- docs/src/examples/spring_mass_system.md | 2 +- docs/src/examples/three_springs.md | 2 +- src/PlanarMechanics/components.jl | 4 +- src/PlanarMechanics/joints.jl | 4 +- src/PlanarMechanics/sensors.jl | 22 +++++------ src/components.jl | 26 ++++++------ src/fancy_joints.jl | 14 +++---- src/frames.jl | 2 +- src/joints.jl | 30 +++++++------- src/orientation.jl | 28 ++++++------- src/robot/FullRobot.jl | 4 +- src/robot/path_planning.jl | 12 +++--- src/robot/robot_components.jl | 20 +++++----- src/sensors.jl | 20 +++++----- src/wheels.jl | 14 +++---- test/runtests.jl | 48 +++++++++++------------ test/test_fourbar.jl | 4 +- 27 files changed, 150 insertions(+), 150 deletions(-) diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index b9085a4c..515d8c4e 100644 --- a/docs/src/examples/free_motion.md +++ b/docs/src/examples/free_motion.md @@ -18,7 +18,7 @@ world = Multibody.world eqs = [connect(world.frame_b, freeMotion.frame_a) connect(freeMotion.frame_b, body.frame_a)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [world; freeMotion; body]) @@ -60,7 +60,7 @@ eqs = [connect(bar2.frame_a, world.frame_b) connect(spring1.frame_a, world.frame_b) connect(body.frame_b, spring2.frame_b)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [ world, body, diff --git a/docs/src/examples/gearbox.md b/docs/src/examples/gearbox.md index a34a2709..70aa22d2 100644 --- a/docs/src/examples/gearbox.md +++ b/docs/src/examples/gearbox.md @@ -46,7 +46,7 @@ eqs = [connect(world.frame_b, gearConstraint.bearing) connect(mounting1D.flange_b, torque2.support) connect(fixed.frame_b, mounting1D.frame_a)] -@named model = ODESystem(eqs, t, systems = [world; systems]) +@named model = System(eqs, t, systems = [world; systems]) cm = complete(model) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ diff --git a/docs/src/examples/gyroscopic_effects.md b/docs/src/examples/gyroscopic_effects.md index d91e4b37..fa6d3f66 100644 --- a/docs/src/examples/gyroscopic_effects.md +++ b/docs/src/examples/gyroscopic_effects.md @@ -36,7 +36,7 @@ connections = [ connect(trans.frame_b, body2.frame_a) ] -@named model = ODESystem(connections, t, systems = [world; systems]) +@named model = System(connections, t, systems = [world; systems]) model = complete(model) ssys = structural_simplify(multibody(model)) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index d572755c..520e51e2 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -73,7 +73,7 @@ connections = [ connect(j2.support, damper2.flange_b) ] -@named fourbar = ODESystem(connections, t, systems = [world; systems]) +@named fourbar = System(connections, t, systems = [world; systems]) fourbar = complete(fourbar) ssys = structural_simplify(multibody(fourbar)) prob = ODEProblem(ssys, [fourbar.j1.phi => 0.1], (0.0, 10.0)) @@ -144,7 +144,7 @@ connections = [connect(j2.frame_b, b2.frame_a) connect(b0.frame_a, world.frame_b) connect(b0.frame_b, j2.frame_a) ] -@named fourbar2 = ODESystem(connections, t, systems = [world; systems]) +@named fourbar2 = System(connections, t, systems = [world; systems]) fourbar2 = complete(fourbar2) ssys = structural_simplify(multibody(fourbar2)) @@ -185,7 +185,7 @@ connections = [connect(j2.frame_b, b2.frame_a) connect(b3.frame_b, j2.frame_a) ] -@named fourbar_analytic = ODESystem(connections, t, systems = [world; systems]) +@named fourbar_analytic = System(connections, t, systems = [world; systems]) fourbar_analytic = complete(fourbar_analytic) ssys_analytic = structural_simplify(multibody(fourbar_analytic)) prob = ODEProblem(ssys_analytic, [], (0.0, 1.4399)) diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 4c048812..ec5e49c4 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -39,13 +39,13 @@ connections = [ nothing # hide ``` -With all components and connections defined, we can create an `ODESystem` like so: +With all components and connections defined, we can create an `System` like so: ```@example pendulum @named model = System(connections, t, systems=[world, joint, body]) model = complete(model) nothing # hide ``` -The `ODESystem` is the fundamental model type in ModelingToolkit used for multibody-type models. +The `System` is the fundamental model type in ModelingToolkit used for multibody-type models. Before we can simulate the system, we must perform model check using the function [`multibody`](@ref) and compilation using [`structural_simplify`](@ref) ```@example pendulum @@ -92,7 +92,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.support, damper.flange_a) connect(body.frame_a, joint.frame_b)] -@named model = ODESystem(connections, t, systems = [world, joint, body, damper]) +@named model = System(connections, t, systems = [world, joint, body, damper]) model = complete(model) ssys = structural_simplify(multibody(model)) @@ -125,7 +125,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.support, damper.flange_a, spring.flange_a) connect(body_0.frame_a, joint.frame_b)] -@named model = ODESystem(connections, t, systems = [world, joint, body_0, damper, spring]) +@named model = System(connections, t, systems = [world, joint, body_0, damper, spring]) model = complete(model) ssys = structural_simplify(multibody(model)) @@ -152,7 +152,7 @@ In the example above, we introduced a prismatic joint to model the oscillating m connections = [connect(world.frame_b, multibody_spring.frame_a) connect(root_body.frame_a, multibody_spring.frame_b)] -@named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body]) +@named model = System(connections, t, systems = [world, multibody_spring, root_body]) model = complete(model) ssys = structural_simplify(multibody(model)) @@ -170,7 +170,7 @@ Internally, the [`Multibody.Spring`](@ref) contains a `Translational.Spring`, at push!(connections, connect(multibody_spring.spring2d.flange_a, damper.flange_a)) push!(connections, connect(multibody_spring.spring2d.flange_b, damper.flange_b)) -@named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body, damper]) +@named model = System(connections, t, systems = [world, multibody_spring, root_body, damper]) model = complete(model) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, defs, (0, 10)) @@ -426,7 +426,7 @@ lsys = named_ss(multibody(cp), inputs, outputs; op) # identical to linearize, bu ### LQR and LQG Control design With a linear statespace object in hand, we can proceed to design an LQR or LQG controller. We will design both an LQR and an LQG controller in order to demonstrate two possible workflows. -The LQR controller is designed using the function `ControlSystemsBase.lqr`, and it takes the two cost matrices `Q1` and `Q2` penalizing state deviation and control action respectively. The LQG controller is designed using [`RobustAndOptimalControl.LQGProblem`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/#LQG-design), and this function additionally takes the covariance matrices `r1, R2` for a Kalman filter. Before we call `LQGProblem` we partition the linearized system into an [`ExtendedStateSpace`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/api/#RobustAndOptimalControl.ExtendedStateSpace) object, this indicates which inputs of the system are available for control and which are considered disturbances, and which outputs of the system are available for measurement. In this case, we assume that we have access to the cart position and the pendulum angle, and we control the cart position. The remaining two outputs are still important for the performance, but we cannot measure them and will rely on the Kalman filter to estimate them. When we call [`extended_controller`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/api/#RobustAndOptimalControl.extended_controller) we get a linear system that represents the combined state estimator and state feedback controller. This linear system is then converted to an `ODESystem` by the function `LQGSystem`. +The LQR controller is designed using the function `ControlSystemsBase.lqr`, and it takes the two cost matrices `Q1` and `Q2` penalizing state deviation and control action respectively. The LQG controller is designed using [`RobustAndOptimalControl.LQGProblem`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/#LQG-design), and this function additionally takes the covariance matrices `r1, R2` for a Kalman filter. Before we call `LQGProblem` we partition the linearized system into an [`ExtendedStateSpace`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/api/#RobustAndOptimalControl.ExtendedStateSpace) object, this indicates which inputs of the system are available for control and which are considered disturbances, and which outputs of the system are available for measurement. In this case, we assume that we have access to the cart position and the pendulum angle, and we control the cart position. The remaining two outputs are still important for the performance, but we cannot measure them and will rely on the Kalman filter to estimate them. When we call [`extended_controller`](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/api/#RobustAndOptimalControl.extended_controller) we get a linear system that represents the combined state estimator and state feedback controller. This linear system is then converted to an `System` by the function `LQGSystem`. Since the function `lqr` operates on the state vector, and we have access to the specified output vector, we make use of the system ``C`` matrix to reformulate the problem in terms of the outputs. This relies on the ``C`` matrix being full rank, which is the case here since our outputs include a complete state realization of the system. This is of no concern when using the `LQGProblem` structure since we penalize outputs rather than the state in this case. @@ -449,7 +449,7 @@ z = [:x] # The output for which we want to have unit static gain Ce, cl = extended_controller(lqg, z = RobustAndOptimalControl.names2indices(z, Pn.y)) dc_gain_compensation = inv((Pn[:x, :].C*dcgain(cl)')[]) # Multiplier that makes the static gain from references to cart position unity -LQGSystem(args...; kwargs...) = ODESystem(Ce; kwargs...) +LQGSystem(args...; kwargs...) = System(Ce; kwargs...) @mtkmodel CartWithFeedback begin @components begin diff --git a/docs/src/examples/quad.md b/docs/src/examples/quad.md index 7cfbea48..f53d05aa 100644 --- a/docs/src/examples/quad.md +++ b/docs/src/examples/quad.md @@ -183,7 +183,7 @@ function RotorCraft(; closed_loop = true, addload=true, L=nothing, outputs = not end end - @named model = ODESystem(connections, t; systems) + @named model = System(connections, t; systems) complete(model) end model = RotorCraft(closed_loop=true, addload=true, pid=true) diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index 404753ae..bd0d34d2 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -28,7 +28,7 @@ number_of_links = 6 connections = [connect(world.frame_b, rope.frame_a) connect(rope.frame_b, body.frame_a)] -@named stiff_rope = ODESystem(connections, t, systems = [world, body, rope]) +@named stiff_rope = System(connections, t, systems = [world, body, rope]) stiff_rope = complete(stiff_rope) ssys = structural_simplify(multibody(stiff_rope)) @@ -52,7 +52,7 @@ number_of_links = 6 connections = [connect(world.frame_b, rope.frame_a) connect(rope.frame_b, body.frame_a)] -@named flexible_rope = ODESystem(connections, t, systems = [world, body, rope]) +@named flexible_rope = System(connections, t, systems = [world, body, rope]) flexible_rope = complete(flexible_rope) ssys = structural_simplify(multibody(flexible_rope)) @@ -86,7 +86,7 @@ connections = [connect(world.frame_b, fixed.frame_a, chain.frame_a) connect(chain.frame_b, spring.frame_a) connect(spring.frame_b, fixed.frame_b)] -@named mounted_chain = ODESystem(connections, t, systems = [systems; world]) +@named mounted_chain = System(connections, t, systems = [systems; world]) mounted_chain = complete(mounted_chain) ssys = structural_simplify(multibody(mounted_chain)) prob = ODEProblem(ssys, [ diff --git a/docs/src/examples/sensors.md b/docs/src/examples/sensors.md index 3e3c083f..c0e4e714 100644 --- a/docs/src/examples/sensors.md +++ b/docs/src/examples/sensors.md @@ -29,7 +29,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(torquesensor.frame_b, forcesensor.frame_a) connect(forcesensor.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [world, joint, body, torquesensor, forcesensor]) ssys = structural_simplify(multibody(model)) diff --git a/docs/src/examples/spherical_pendulum.md b/docs/src/examples/spherical_pendulum.md index 4fdc4906..6990909c 100644 --- a/docs/src/examples/spherical_pendulum.md +++ b/docs/src/examples/spherical_pendulum.md @@ -25,7 +25,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, bar.frame_a) connect(bar.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, systems = [world; systems]) +@named model = System(connections, t, systems = [world; systems]) model = complete(model) ssys = structural_simplify(multibody(model)) diff --git a/docs/src/examples/spring_damper_system.md b/docs/src/examples/spring_damper_system.md index e8ad18f7..db8d5dfd 100644 --- a/docs/src/examples/spring_damper_system.md +++ b/docs/src/examples/spring_damper_system.md @@ -40,7 +40,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) connect(damper1.frame_b, body1.frame_a) connect(spring1.frame_b, body1.frame_a)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [ world, body1, diff --git a/docs/src/examples/spring_mass_system.md b/docs/src/examples/spring_mass_system.md index ac2084e9..2eddd0a3 100644 --- a/docs/src/examples/spring_mass_system.md +++ b/docs/src/examples/spring_mass_system.md @@ -44,7 +44,7 @@ eqs = [ connect(spring1.flange_a, p1.support) ] -@named model = ODESystem(eqs, t, systems = [world; systems]) +@named model = System(eqs, t, systems = [world; systems]) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys,[], (0, 5)) diff --git a/docs/src/examples/three_springs.md b/docs/src/examples/three_springs.md index 7d3072d1..6ba93969 100644 --- a/docs/src/examples/three_springs.md +++ b/docs/src/examples/three_springs.md @@ -36,7 +36,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) connect(spring3.frame_b, spring1.frame_b) connect(spring2.frame_a, spring1.frame_b)] -@named model = ODESystem(eqs, t, systems = [world; systems]) +@named model = System(eqs, t, systems = [world; systems]) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0, 10)) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 7f639252..f0afa2ff 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -90,7 +90,7 @@ Body component with mass and inertia I * α ~ frame_a.tau ] - return compose(ODESystem(eqs, t, vars, pars; name), + return compose(System(eqs, t, vars, pars; name), frame_a) end @@ -732,7 +732,7 @@ In addition there is an input `dynamicLoad` for a dynamic component of the norma f_lat ~ [frame_a.fy, -frame_a.fx]'e0 ] - return ODESystem(equations, t, vars, pars; name, systems) + return System(equations, t, vars, pars; name, systems) end diff --git a/src/PlanarMechanics/joints.jl b/src/PlanarMechanics/joints.jl index e543e27f..330a250d 100644 --- a/src/PlanarMechanics/joints.jl +++ b/src/PlanarMechanics/joints.jl @@ -77,7 +77,7 @@ A revolute joint end - return compose(ODESystem(eqs, t, vars, pars; name = name), + return compose(System(eqs, t, vars, pars; name = name), systems...) end @@ -178,5 +178,5 @@ A prismatic joint # actutation torque push!(eqs, f ~ 0) end - return extend(ODESystem(eqs, t, vars, pars; name, systems), partial_frames) + return extend(System(eqs, t, vars, pars; name, systems), partial_frames) end diff --git a/src/PlanarMechanics/sensors.jl b/src/PlanarMechanics/sensors.jl index 56e3d56c..8b4aa756 100644 --- a/src/PlanarMechanics/sensors.jl +++ b/src/PlanarMechanics/sensors.jl @@ -141,7 +141,7 @@ Measure absolute position and orientation (same as Sensors.AbsolutePosition, but frame_a.tau ~ 0 ] - return compose(ODESystem(eqs, t, [], []; name = name), + return compose(System(eqs, t, [], []; name = name), x, y, phi, frame_a, frame_resolve) end @@ -189,7 +189,7 @@ Measure absolute position and orientation of the origin of frame connector push!(eqs, connect(zero_position.frame_resolve, pos.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end """ @@ -253,7 +253,7 @@ Measure relative position and orientation between the origins of two frame conne frame_b.tau ~ 0 ] - return compose(ODESystem(eqs, t, [], []; name = name), + return compose(System(eqs, t, [], []; name = name), rel_x, rel_y, rel_phi, frame_a, frame_b, frame_resolve) end @@ -300,7 +300,7 @@ Measure relative position and orientation between the origins of two frame conne push!(eqs, connect(zero_position.frame_resolve, pos.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), + return compose(System(eqs, t, [], []; name = name), systems...) end @@ -391,7 +391,7 @@ end end end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end @component function TransformAbsoluteVector(; @@ -436,7 +436,7 @@ end connect(zero_pos.frame_resolve, basic_transform_vector.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end @component function AbsoluteVelocity(; name, resolve_in_frame = :frame_a) @@ -484,7 +484,7 @@ end zero_pos1.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end @component function BasicTransformRelativeVector(; @@ -569,7 +569,7 @@ end ]) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end @component function TransformRelativeVector(; @@ -624,7 +624,7 @@ end push!(systems, zero_pos) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end @component function RelativeVelocity(; name, resolve_in_frame = :frame_a) @@ -679,7 +679,7 @@ end zero_pos.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end @component function AbsoluteAcceleration(; name, resolve_in_frame = :frame_a) @@ -785,7 +785,7 @@ end zero_pos.frame_resolve)) end - return compose(ODESystem(eqs, t, [], []; name = name), systems...) + return compose(System(eqs, t, [], []; name = name), systems...) end function connect_sensor(component_frame, sensor_frame) diff --git a/src/components.jl b/src/components.jl index abdc23f8..4685e77d 100644 --- a/src/components.jl +++ b/src/components.jl @@ -94,7 +94,7 @@ If a connection to the world is needed in a component model, use [`Fixed`](@ref) render_inner ~ render point_gravity_inner ~ point_gravity ] - ODESystem(eqs, t, [n_inner; g_inner; mu_inner; render_inner; point_gravity_inner], [n; g; mu; point_gravity; render]; name, systems = [frame_b])#, defaults=[n => n0; g => g0; mu => mu0]) + System(eqs, t, [n_inner; g_inner; mu_inner; render_inner; point_gravity_inner], [n; g; mu; point_gravity; render]; name, systems = [frame_b])#, defaults=[n => n0; g => g0; mu => mu0]) end """ @@ -128,7 +128,7 @@ A component rigidly attached to the world frame with translation `r` between the end eqs = [collect(frame_b.r_0 .~ r) ori(frame_b) ~ nullrotation()] - sys = compose(ODESystem(eqs, t; name=:nothing), systems...) + sys = compose(System(eqs, t; name=:nothing), systems...) add_params(sys, [render]; name) end @@ -163,7 +163,7 @@ See also [`Pose`](@ref) for a component that allows for forced orientation as we if fixed_orientation append!(eqs, ori(frame_b) ~ nullrotation()) end - sys = compose(ODESystem(eqs, t; name=:nothing), systems...) + sys = compose(System(eqs, t; name=:nothing), systems...) add_params(sys, [render]; name) end @@ -215,7 +215,7 @@ See also [`Position`](@ref) for a component that allows for only forced translat error("Either R or q must be provided. If you only want to specify the position, use the `Position` component instead.") end ] - sys = compose(ODESystem(eqs, t; name=:nothing), systems...) + sys = compose(System(eqs, t; name=:nothing), systems...) add_params(sys, [render]; name) end @@ -239,7 +239,7 @@ end eqs = [(collect(housing_tau) .~ collect(-n * flange_b.tau)); (flange_b.phi ~ phi0); connect(housing_frame_a, frame_a)] - compose(ODESystem(eqs, t; name), systems...) + compose(System(eqs, t; name), systems...) end """ @@ -271,7 +271,7 @@ Can be thought of as a massless rod. For a massive rod, see [`BodyShape`](@ref) (0 .~ taua + taub + cross(r, fb))] pars = [r; radius; color; render] vars = [] - compose(ODESystem(eqs, t, vars, pars; name), frame_a, frame_b) + compose(System(eqs, t, vars, pars; name), frame_a, frame_b) end """ @@ -334,7 +334,7 @@ To obtain an axis-angle representation of any rotation, see [Conversion between eqs = collect(eqs) append!(eqs, collect(frame_b.r_0) .~ collect(frame_a.r_0) + resolve1(frame_a, r)) - compose(ODESystem(eqs, t, [], pars; name), frame_a, frame_b) + compose(System(eqs, t, [], pars; name), frame_a, frame_b) end """ @@ -505,7 +505,7 @@ This component has a single frame, `frame_a`. To represent bodies with more than # 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]) + sys = System(eqs, t; name=:nothing, metadata = Dict(IsRoot => isroot), systems = [frame_a]) add_params(sys, [radius; cylinder_radius; color; length_fraction; render]; name) end @@ -591,7 +591,7 @@ See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with p connect(frame_a, body.frame_a) connect(frame_cm, translation_cm.frame_b) ] - ODESystem(eqs, t, [r_0; v_0; a_0], pars; name, systems) + System(eqs, t, [r_0; v_0; a_0], pars; name, systems) end @@ -674,7 +674,7 @@ function Rope(; name, l = 1, dir = [0,-1, 0], n = 10, m = 1, c = 0, d=0, air_res push!(eqs, connect(links[i].frame_b, joints[i+1].frame_a)) end - ODESystem(eqs, t; name, systems = [systems; links; joints]) + System(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...) @@ -763,8 +763,8 @@ end # # 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) +# # System(eqs, t, vars, pars; name, systems) +# System(eqs, t; name, systems) # end """ @@ -1127,5 +1127,5 @@ end # connect(frame_b, translation.frame_b) # connect(frame_a, body.frame_a) # ] -# ODESystem(equations, t, vars, pars; name, systems) +# System(equations, t, vars, pars; name, systems) # end diff --git a/src/fancy_joints.jl b/src/fancy_joints.jl index 0a0ffe22..c389c7a3 100644 --- a/src/fancy_joints.jl +++ b/src/fancy_joints.jl @@ -124,7 +124,7 @@ It is not possible to connect other components, such as a body with mass propert ] - sys = extend(ODESystem(eqs, t; name=:nothing), ptf) + sys = extend(System(eqs, t; name=:nothing), ptf) add_params(sys, pars; name) end @@ -195,7 +195,7 @@ In complex multibody systems with closed loops this may help to simplify the sys zeros(3) .~ collect(frame_a.tau) + resolve1(Rrel, frame_b.tau) - cross(r_rel_a, frame_a.f); ] - sys = extend(ODESystem(eqs, t; name=:nothing), ptf) + sys = extend(System(eqs, t; name=:nothing), ptf) add_params(sys, pars; name) end @@ -259,7 +259,7 @@ In systems without closed loops the use of this implicit joint does not make sen Main.eqs = eqs - sys = extend(ODESystem(eqs, t, collect(r_rel_a), pars; name), ptf) + sys = extend(System(eqs, t, collect(r_rel_a), pars; name), ptf) end @@ -436,7 +436,7 @@ This joint aggregation can be used in cases where in reality a rod with spherica residue === :external || error("Unknown value for constraint_residue, expected nothing or :external") end - sys = ODESystem(eqs, t; name=:nothing, systems) + sys = System(eqs, t; name=:nothing, systems) add_params(sys, pars; name) end @@ -520,7 +520,7 @@ end end ] - sys = ODESystem(eqs, t; name=:nothing, systems)#, parameter_dependencies = [positive_branch => select_branch(length_constraint, e, phi_offset + phi_guess, r_a, r_b)]) # JuliaSimCompiler ignores parameter dependencies, the user has to provide it instead + sys = System(eqs, t; name=:nothing, systems)#, parameter_dependencies = [positive_branch => select_branch(length_constraint, e, phi_offset + phi_guess, r_a, r_b)]) # JuliaSimCompiler ignores parameter dependencies, the user has to provide it instead add_params(sys, pars; name) end @@ -666,7 +666,7 @@ The rest of this joint aggregation is defined by the following parameters: connect(relative_position.r_rel, revolute.position_a) connect(revolute.bearing, bearing) ] - ODESystem(eqs, t; name, systems=[systems; more_systems]) + System(eqs, t; name, systems=[systems; more_systems]) end @@ -777,7 +777,7 @@ Basically, the JointRRR model internally consists of a universal-spherical-revol connect(jointUSR.bearing, bearing) ] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end diff --git a/src/frames.jl b/src/frames.jl index 1368581d..1002829e 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -21,7 +21,7 @@ R = NumRotationMatrix(; name, varw, state_priority=-1) - ODESystem(Equation[], t, [r_0; f; tau; vec(R.R)], pars; name, + System(Equation[], t, [r_0; f; tau; vec(R.R)], pars; name, metadata = Dict(ModelingToolkit.FrameOrientation => R, ModelingToolkit.IsFrame => true)) end diff --git a/src/joints.jl b/src/joints.jl index eeef483e..e48a92d9 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -1,6 +1,6 @@ function add_params(sys, params; name) params isa AbstractVector || (params = [params...]) - extend(ODESystem(Equation[], t, [], params; name), sys) + extend(System(Equation[], t, [], params; name), sys) end """ @@ -77,13 +77,13 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Rotatio push!(eqs, axis.phi ~ phi) push!(eqs, axis.tau ~ tau) # push!(eqs, connect(internalAxis.flange, axis)) - ODESystem(eqs, t; name=:nothing, systems=[frame_a, frame_b, axis, support, fixed]) + System(eqs, t; name=:nothing, systems=[frame_a, frame_b, axis, support, fixed]) else # Modelica Revolute uses a ConstantTorque as well as internalAxis = Rotational.InternalSupport(tau=tau), but it seemed more complicated than required and I couldn't get it to work, likely due to the `input` semantics of modelica not having an equivalent in MTK, so the (tau=tau) input argument caused problems. # @named constantTorque = Rotational.ConstantTorque(tau_constant=0, use_support=false) # push!(eqs, connect(constantTorque.flange, internalAxis.flange)) push!(eqs, tau ~ 0) - ODESystem(eqs, t; name=:nothing, systems=[frame_a, frame_b]) + System(eqs, t; name=:nothing, systems=[frame_a, frame_b]) end add_params(sys, pars; name) end @@ -100,7 +100,7 @@ If `axisflange`, flange connectors for ModelicaStandardLibrary.Mechanics.Transla - `axis`: 1-dim. translational flange that drives the joint - `support`: 1-dim. translational flange of the drive support (assumed to be fixed in the world frame, NOT in the joint) -The function returns an ODESystem representing the prismatic joint. +The function returns an System representing the prismatic joint. """ @component function Prismatic(; name, n = Float64[0, 0, 1], axisflange = false, s0 = 0, v0 = 0, radius = 0.05, color = [0,0.8,1,1], state_priority=10, iscut=false, render=true) @@ -155,10 +155,10 @@ The function returns an ODESystem representing the prismatic joint. push!(eqs, connect(fixed.flange, support)) push!(eqs, axis.s ~ s) push!(eqs, axis.f ~ f) - compose(ODESystem(eqs, t; name=:nothing), frame_a, frame_b, axis, support, fixed) + compose(System(eqs, t; name=:nothing), frame_a, frame_b, axis, support, fixed) else push!(eqs, f ~ 0) - compose(ODESystem(eqs, t; name=:nothing), frame_a, frame_b) + compose(System(eqs, t; name=:nothing), frame_a, frame_b) end add_params(sys, pars; name) end @@ -267,7 +267,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin end end - sys = extend(ODESystem(eqs, t; name=:nothing), ptf) + sys = extend(System(eqs, t; name=:nothing), ptf) add_params(sys, pars; name) end @@ -350,7 +350,7 @@ Joint where `frame_a` rotates around axis `n_a` which is fixed in `frame_a` and connect(frame_a, revolute_a.frame_a) connect(revolute_b.frame_b, frame_b) connect(revolute_a.frame_b, revolute_b.frame_a)] - extend(ODESystem(eqs, t; name, systems = [revolute_a, revolute_b]), ptf) + extend(System(eqs, t; name, systems = [revolute_a, revolute_b]), ptf) end """ @@ -447,7 +447,7 @@ This ideal massless joint provides a gear constraint between frames `frame_a` an bearing.tau'angular_velocity2(bearing)) end - extend(ODESystem(eqs, t; name, systems), ptf) + extend(System(eqs, t; name, systems), ptf) end @@ -564,11 +564,11 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi end end if state && !isroot - compose(ODESystem(eqs, t; name), frame_a, frame_b, Rrel_f, Rrel_inv_f) + compose(System(eqs, t; name), frame_a, frame_b, Rrel_f, Rrel_inv_f) elseif state - compose(ODESystem(eqs, t; name), frame_a, frame_b, Rrel_f, ) + compose(System(eqs, t; name), frame_a, frame_b, Rrel_f, ) else - compose(ODESystem(eqs, t; name), frame_a, frame_b) + compose(System(eqs, t; name), frame_a, frame_b) end end @@ -642,7 +642,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi collect(frame_b.f) .~ -resolve2(Rrel, frame_a.f) collect(n) .~ n0 ] - sys = ODESystem(eqs, t; name=:nothing, systems=[frame_a, frame_b]) + sys = System(eqs, t; name=:nothing, systems=[frame_a, frame_b]) add_params(sys, pars; name) end @@ -843,7 +843,7 @@ end connect(support, rev.support) ] end - ODESystem(connections, t; systems, name) + System(connections, t; systems, name) end @component function URDFPrismatic(; name, r, R, axisflange = false, kwargs...) @@ -882,7 +882,7 @@ end connect(support, rev.support) ] end - ODESystem(connections, t; systems, name) + System(connections, t; systems, name) end @mtkmodel NullJoint begin diff --git a/src/orientation.jl b/src/orientation.jl index 19ef65e4..e46bad2b 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -12,7 +12,7 @@ abstract type Orientation end A struct representing a 3D orientation as a rotation matrix. -If `ODESystem` is called on a `RotationMatrix` object `o`, symbolic variables for `o.R` and `o.w` are created and the value of `o.R` is used as the default value for the symbolic `R`. +If `System` is called on a `RotationMatrix` object `o`, symbolic variables for `o.R` and `o.w` are created and the value of `o.R` is used as the default value for the symbolic `R`. # Fields: - `R::R3`: The rotation 3×3 matrix ∈ SO(3) @@ -56,7 +56,7 @@ end nullrotation() = RotationMatrix() -function ModelingToolkit.ODESystem(RM::RotationMatrix; name) +function ModelingToolkit.System(RM::RotationMatrix; name) # @variables R(t)[1:3, 1:3]=Matrix(RM) [description="Orientation rotation matrix ∈ SO(3)"] # @variables w(t)[1:3]=w [description="angular velocity"] # R,w = collect.((R,w)) @@ -65,7 +65,7 @@ function ModelingToolkit.ODESystem(RM::RotationMatrix; name) w = at_variables_t(:w, 1:3) defaults = Dict(R .=> RM) - ODESystem(Equation[], t, [vec(R); w], []; name, defaults) + System(Equation[], t, [vec(R); w], []; name, defaults) end Base.:*(R1::RotationMatrix, x::AbstractArray) = R1.R * x @@ -133,12 +133,12 @@ r_wb = resolve1(ori(frame_a), r_ab) """ resolve1(R21::RotationMatrix, v2) = R21'collect(v2) -resolve1(sys::ODESystem, v) = resolve1(ori(sys), v) -resolve2(sys::ODESystem, v) = resolve2(ori(sys), v) +resolve1(sys::System, v) = resolve1(ori(sys), v) +resolve2(sys::System, v) = resolve2(ori(sys), v) function resolve_relative(v1, R1, R2) - R1 isa ODESystem && (R1 = ori(R1)) - R2 isa ODESystem && (R2 = ori(R2)) + R1 isa System && (R1 = ori(R1)) + R2 isa System && (R2 = ori(R2)) v2 = resolve2(R2, resolve1(R1, v1)) end @@ -171,21 +171,21 @@ function absolute_rotation(R1, Rrel) # R2 = Rrel.R*R1.R # w = resolve2(Rrel, R1.w) + Rrel.w # RotationMatrix(R2, w) - R1 isa ODESystem && (R1 = ori(R1)) - Rrel isa ODESystem && (Rrel = ori(Rrel)) + R1 isa System && (R1 = ori(R1)) + Rrel isa System && (Rrel = ori(Rrel)) Rrel * R1 end function relative_rotation(R1, R2) - R1 isa ODESystem && (R1 = ori(R1)) - R2 isa ODESystem && (R2 = ori(R2)) + R1 isa System && (R1 = ori(R1)) + R2 isa System && (R2 = ori(R2)) R = R2'R1 w = R2.w - resolve2(R2, resolve1(R1, R1.w)) RotationMatrix(R.R, w) end function inverse_rotation(R) - R isa ODESystem && (R = ori(R)) + R isa System && (R = ori(R)) Ri = R.R' wi = -resolve1(R, R.w) RotationMatrix(Ri, wi) @@ -232,8 +232,8 @@ orientation_constraint(R1, R2) = orientation_constraint(R1'R2) function residue(R1, R2) # https://github.com/modelica/ModelicaStandardLibrary/blob/master/Modelica/Mechanics/MultiBody/Frames/Orientation.mo - R1 isa ODESystem && (R1 = ori(R1)) - R2 isa ODESystem && (R2 = ori(R2)) + R1 isa System && (R1 = ori(R1)) + R2 isa System && (R2 = ori(R2)) R1 = R1.R R2 = R2.R [atan(cross(R1[1, :], R1[2, :]) ⋅ R2[2, :], R1[1, :] ⋅ R2[1, :]) diff --git a/src/robot/FullRobot.jl b/src/robot/FullRobot.jl index 983f48de..2c168995 100644 --- a/src/robot/FullRobot.jl +++ b/src/robot/FullRobot.jl @@ -66,7 +66,7 @@ function RobotAxis(; name, mLoad = 15, kp = 5.0, ks = 0.5, Ts = 0.05, q0 = 0, connect(pathPlanning.controlBus, controlBus), connect(controlBus.axisControlBus1, axis.axisControlBus), ] - ODESystem(eqs, t; systems, name) + System(eqs, t; systems, name) end @@ -262,5 +262,5 @@ function Robot6DOF(; name, kwargs...) connect(controlBus.axisControlBus5, axis5.axisControlBus) connect(controlBus.axisControlBus6, axis6.axisControlBus)] - ODESystem(eqs, t; systems, name) + System(eqs, t; systems, name) end diff --git a/src/robot/path_planning.jl b/src/robot/path_planning.jl index 81a122f4..01f34fde 100644 --- a/src/robot/path_planning.jl +++ b/src/robot/path_planning.jl @@ -21,7 +21,7 @@ function PathPlanning1(; name, q0deg = 0, q1deg = 1, time = 0:0.01:10, speed_max connect(path.qdd, pathToAxis1.qdd) # connect(path.moving, pathToAxis1.moving) connect(pathToAxis1.axisControlBus, controlBus.axisControlBus1)] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end function PathPlanning6(; name, naxis = 6, q0deg = zeros(naxis), @@ -75,7 +75,7 @@ function PathPlanning6(; name, naxis = 6, q0deg = zeros(naxis), connect(pathToAxis5.axisControlBus, controlBus.axisControlBus5) connect(pathToAxis6.axisControlBus, controlBus.axisControlBus6)] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end "Map path planning to one axis control bus" @@ -106,7 +106,7 @@ function PathToAxisControlBus(; name, nAxis = 6, axisUsed = 1) (qdd_axisUsed.output.u ~ axisControlBus.acceleration_ref) (qd_axisUsed.output.u ~ axisControlBus.speed_ref) (q_axisUsed.output.u ~ axisControlBus.angle_ref)] - ODESystem(eqs, t; systems, name) + System(eqs, t; systems, name) end """ @@ -193,7 +193,7 @@ function KinematicPTP(; time, name, q0 = 0, q1 = 1, qd_max=1, qdd_max=1) qdd.u[i] ~ qddfun(t)] end eqs = reduce(vcat, interp_eqs) - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end @@ -233,7 +233,7 @@ function Kinematic5(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0, end eqs = reduce(vcat, interp_eqs) - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end """ @@ -249,5 +249,5 @@ Pass a Real signal through without modification @named siso = Blocks.SISO() @unpack u, y = siso eqs = [y ~ u] - extend(ODESystem(eqs, t; name), siso) + extend(System(eqs, t; name), siso) end diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index c0751a89..ed9d728d 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -33,7 +33,7 @@ D = Differential(t) (motorAngle(t)), [guess=0.0,output = true, description = "Angle of motor flange"] (motorSpeed(t)), [guess=0.0,output = true, description = "Speed of motor flange"] end - ODESystem(Equation[], t, vars, []; name) + System(Equation[], t, vars, []; name) end @connector function ControlBus(; name) @@ -45,7 +45,7 @@ end axisControlBus5 = AxisControlBus() axisControlBus6 = AxisControlBus() end - ODESystem(Equation[], t; systems, name) + System(Equation[], t; systems, name) end """ @@ -66,7 +66,7 @@ Ideal rotational sensor to measure the absolute flange angular acceleration a.u ~ D(w) flange.tau ~ 0 ] - return ODESystem(eqs, t, [], []; name = name, systems = [flange, a]) + return System(eqs, t, [], []; name = name, systems = [flange, a]) end RotationalFlange = Rotational.Flange @@ -135,7 +135,7 @@ function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = (accSensor.a.u/ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus)] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end @@ -170,7 +170,7 @@ function AxisType1(; name, c = 43, cd = 0.005, kp = 10, ks = 1, Ts = 0.01, k = 1 connect(controller.axisControlBus, axisControlBus) ] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end function Controller(; name, kp = 10, ks = 1, Ts = 0.01, ratio = 1) @@ -201,7 +201,7 @@ function Controller(; name, kp = 10, ks = 1, Ts = 0.01, ratio = 1) (add3.input3.u ~ axisControlBus.motorSpeed) (PI.ctr_output.u ~ axisControlBus.current_ref)] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end function GearType2(; name, i = -99, @@ -244,7 +244,7 @@ function GearType2(; name, i = -99, # connect(bearingFriction.flange_b, flange_b) # connect(bearingFriction.flange_a, flange_a) ] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end import ModelingToolkitStandardLibrary.Mechanical.Rotational.Flange as fl @@ -295,7 +295,7 @@ function GearType1(; name, i = -105, c = 43, d = 0.005, connect(bearingFriction.flange_b, spring.flange_a) connect(gear.flange_b, flange_b) connect(bearingFriction.flange_a, flange_a)] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end function Motor(; name, J = 0.0013, k = 1.1616, w = 4590, D = 0.6, w_max = 315, i_max = 9) @@ -384,7 +384,7 @@ function Motor(; name, J = 0.0013, k = 1.1616, w = 4590, D = 0.6, w_max = 315, i (convert2.y ~ Vs.v) connect(emf.flange, Jmotor.flange_a)] - compose(ODESystem(eqs, t; name), systems) + compose(System(eqs, t; name), systems) end robot_orange = [1, 0.51, 0, 1] @@ -546,5 +546,5 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) connect(r6.axis, axis6) connect(r6.frame_b, b6.frame_a)] - compose(ODESystem(eqs, t; name), systems) + compose(System(eqs, t; name), systems) end diff --git a/src/sensors.jl b/src/sensors.jl index 1c89221f..34906215 100644 --- a/src/sensors.jl +++ b/src/sensors.jl @@ -8,7 +8,7 @@ function PartialRelativeBaseSensor(; name) frame_a.tau .~ zeros(3) |> collect frame_b.f .~ zeros(3) |> collect frame_b.tau .~ zeros(3) |> collect] - compose(ODESystem(equations, t; name), frame_a, frame_b) + compose(System(equations, t; name), frame_a, frame_b) end function BasicRelativePosition(; name, resolve_frame) @@ -29,7 +29,7 @@ function BasicRelativePosition(; name, resolve_frame) error("resolve_frame must be :world, :frame_a or :frame_b, you provided $resolve_frame, which makes me sad.") end - extend(compose(ODESystem(eqs, t; name), r_rel), prb) + extend(compose(System(eqs, t; name), r_rel), prb) end function RelativePosition(; name, resolve_frame = :frame_a) @@ -46,7 +46,7 @@ function RelativePosition(; name, resolve_frame = :frame_a) connect(relativePosition.frame_b, frame_b) connect(relativePosition.r_rel, r_rel) ] - compose(ODESystem(eqs, t; name), frame_a, frame_b, r_rel, relativePosition) + compose(System(eqs, t; name), frame_a, frame_b, r_rel, relativePosition) end function PartialAbsoluteSensor(; name, n_out) @@ -55,7 +55,7 @@ function PartialAbsoluteSensor(; name, n_out) y = Blocks.RealOutput(nout = n_out) end equations = [] - compose(ODESystem(equations, t; name), frame_a, y) + compose(System(equations, t; name), frame_a, y) end """ @@ -73,7 +73,7 @@ function PartialCutForceBaseSensor(; name, resolve_frame = :frame_a) ori(frame_a) ~ ori(frame_b) zeros(3) .~ frame_a.f + frame_b.f |> collect zeros(3) .~ frame_a.tau + frame_b.tau |> collect] - compose(ODESystem(equations, t; name), frame_a, frame_b) + compose(System(equations, t; name), frame_a, frame_b) end """ @@ -94,7 +94,7 @@ function CutTorque(; name, resolve_frame = :frame_a) else error("resolve_frame must be :world or :frame_a") end - extend(compose(ODESystem(eqs, t; name), torque), pcfbs) + extend(compose(System(eqs, t; name), torque), pcfbs) end """ @@ -115,7 +115,7 @@ function CutForce(; name, resolve_frame = :frame_a) else error("resolve_frame must be :world or :frame_a") end - extend(compose(ODESystem(eqs, t; name), force), pcfbs) + extend(compose(System(eqs, t; name), force), pcfbs) end function RelativeAngles(; name, sequence = [1, 2, 3]) @@ -131,7 +131,7 @@ function RelativeAngles(; name, sequence = [1, 2, 3]) frame_b.tau .~ zeros(3) |> collect Rrel ~ relative_rotation(frame_a, frame_b) angles .~ axes_rotationangles(Rrel, sequence, guessAngle1)] - compose(ODESystem(eqs, t; name), frame_a, frame_b, angles) + compose(System(eqs, t; name), frame_a, frame_b, angles) end function AbsoluteAngles(; name, sequence = [1, 2, 3]) @@ -144,7 +144,7 @@ function AbsoluteAngles(; name, sequence = [1, 2, 3]) eqs = [collect(frame_a.f .~ 0) collect(frame_a.tau .~ 0) angles.u .~ axes_rotationangles(ori(frame_a), [1, 2, 3])] - extend(compose(ODESystem(eqs, t; name)), pas) + extend(compose(System(eqs, t; name)), pas) end @@ -170,5 +170,5 @@ function Power(; name) power.u ~ collect(frame_a.f)'resolve2(frame_a, D.(frame_a.r_0)) + collect(frame_a.tau)'angular_velocity2(ori(frame_a)) ] - ODESystem(eqs, t; name, systems) + System(eqs, t; name, systems) end \ No newline at end of file diff --git a/src/wheels.jl b/src/wheels.jl index 5e9e4d98..64fae562 100644 --- a/src/wheels.jl +++ b/src/wheels.jl @@ -179,7 +179,7 @@ this frame. zeros(3) .~ collect(frame_a.f) + resolve2(Ra, f_wheel_0) zeros(3) .~ collect(frame_a.tau) + resolve2(Ra, cross(delta_0, f_wheel_0))] - sys = compose(ODESystem(equations, t; name=:nothing), frame_a) + sys = compose(System(equations, t; name=:nothing), frame_a) add_params(sys, [color;]; name) end @@ -264,7 +264,7 @@ with the wheel itself. A [`Revolute`](@ref) joint rotationg around `n = [0, 1, 0 collect(wheeljoint.der_angles) .~ collect(der_angles) connect(body.frame_a, frame_a) connect(wheeljoint.frame_a, frame_a)] - compose(ODESystem(equations, t; name), frame_a, wheeljoint, body) + compose(System(equations, t; name), frame_a, wheeljoint, body) end @@ -493,7 +493,7 @@ plot!( # v_slip~mu_A # v_slip~mu_S # ] - compose(ODESystem(equations, t; name), frame_a) + compose(System(equations, t; name), frame_a) end @@ -566,7 +566,7 @@ See [Docs: Wheels](https://help.juliahub.com/multibody/dev/examples/wheel/) collect(wheeljoint.der_angles) .~ collect(der_angles)] ) end - compose(ODESystem(equations, t; name), frame_a, wheeljoint, body) + compose(System(equations, t; name), frame_a, wheeljoint, body) end """ @@ -660,7 +660,7 @@ function RollingConstraintVerticalWheel(; zeros(3) .~ collect(frame_a.f) + resolve2(ori(frame_a), f_wheel_0) zeros(3) .~ collect(frame_a.tau) + resolve2(ori(frame_a), cross(rContact_0, f_wheel_0)) ] - ODESystem(equations, t; name, systems = [frame_a]) + System(equations, t; name, systems = [frame_a]) end @@ -800,7 +800,7 @@ function RollingWheelSetJoint(; connect(frame_middle, mounting1D.frame_a) connect(mounting1D.flange_b, support) ] - sys = ODESystem(equations, t; name=:nothing, systems) + sys = System(equations, t; name=:nothing, systems) add_params(sys, [width_wheel]; name) end @@ -933,7 +933,7 @@ function RollingWheelSet(; connect(wheelSetJoint.frame_middle, frame_middle) ] - sys = ODESystem(equations, t, sts, pars; name, systems) + sys = System(equations, t, sts, pars; name, systems) end diff --git a/test/runtests.jl b/test/runtests.jl index e3076ff5..86e21a9c 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -73,7 +73,7 @@ D = Differential(t) connections = [connect(world.frame_b, spring.frame_a) connect(spring.frame_b, body.frame_a)] - @named model = ODESystem(connections, t, systems = [world, spring, body]) + @named model = System(connections, t, systems = [world, spring, body]) # ssys = structural_simplify(model, allow_parameter = false) @@ -125,7 +125,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(forcesensor.frame_b, powersensor.frame_a) connect(powersensor.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [world, joint, body, torksensor, forcesensor, powersensor, damper]) # ssys = structural_simplify(model, allow_parameter = false) @@ -212,7 +212,7 @@ connections = [connect(world.frame_b, rev.frame_a) connect(rev.support, damper.flange_a) connect(body.frame_a, rev.frame_b)] -@named model = ODESystem(connections, t, systems = [world, rev, body, damper]) +@named model = System(connections, t, systems = [world, rev, body, damper]) # ssys = structural_simplify(model, allow_parameter = false) irsys = multibody(model) @@ -247,7 +247,7 @@ connections = [connect(world.frame_b, rev.frame_a) connect(rev.frame_b, rod.frame_a) connect(rod.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, systems = [world, rev, body, damper, rod]) +@named model = System(connections, t, systems = [world, rev, body, damper, rod]) # modele = ModelingToolkit.expand_connections(model) # ssys = structural_simplify(model, allow_parameter = false) @@ -281,7 +281,7 @@ connections = [connect(world.frame_b, rev.frame_a) connect(rev.frame_b, rod.frame_a) connect(rod.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, systems = [world, rev, body, damper, rod]) +@named model = System(connections, t, systems = [world, rev, body, damper, rod]) # ssys = structural_simplify(model)#, allow_parameter = false) ssys = structural_simplify(multibody(model)) @@ -322,7 +322,7 @@ connections = [connect(damper1.flange_b, rev1.axis) connect(rev2.frame_b, rod2.frame_a) connect(rod2.frame_b, body2.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [ world, rev1, @@ -371,7 +371,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.support, damper.flange_a, spring.flange_a) connect(body.frame_a, joint.frame_b)] -@named model = ODESystem(connections, t, systems = [world, joint, body, damper, spring]) +@named model = System(connections, t, systems = [world, joint, body, damper, spring]) ssys = structural_simplify(multibody(model))#, allow_parameter = false) prob = ODEProblem(ssys, [damper.s_rel => 1, D(joint.s) => 0, D(D(joint.s)) => 0], @@ -419,7 +419,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) connect(springdamper.frame_b, body3.frame_a) ] -@named model = ODESystem(eqs, t; systems) +@named model = System(eqs, t; systems) # ssys = structural_simplify(model, allow_parameter = false) ssys = structural_simplify(multibody(model))#, alias_eliminate = false) @@ -490,7 +490,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) connect(spring3.frame_b, spring1.frame_b) connect(spring2.frame_a, spring1.frame_b)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [ world, body1, @@ -535,7 +535,7 @@ eqs = [connect(bar2.frame_a, world.frame_b) connect(spring1.frame_a, world.frame_b) connect(body.frame_b, spring2.frame_b)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [ world, body, @@ -620,7 +620,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, bar.frame_a) connect(bar.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, systems = [world, joint, bar, body]) +@named model = System(connections, t, systems = [world, joint, bar, body]) # ssys = structural_simplify(model, allow_parameters = false) ssys = structural_simplify(multibody(model)) @test length(unknowns(ssys)) == 6 @@ -661,7 +661,7 @@ end connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, bar.frame_a) connect(bar.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, systems = [world, joint, bar, body]) +@named model = System(connections, t, systems = [world, joint, bar, body]) model = complete(model) ssys = structural_simplify(multibody(model)) @@ -723,7 +723,7 @@ eqs = [connect(world.frame_b, gearConstraint.bearing) connect(mounting1D.flange_b, torque2.support) connect(fixed.frame_b, mounting1D.frame_a)] -@named model = ODESystem(eqs, t, systems = [world; systems]) +@named model = System(eqs, t, systems = [world; systems]) cm = complete(model) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ @@ -755,7 +755,7 @@ world = Multibody.world eqs = [connect(world.frame_b, freeMotion.frame_a) connect(freeMotion.frame_b, body.frame_a)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [world; freeMotion; body]) @@ -779,7 +779,7 @@ y = sol(0:0.1:10, idxs = body.r_0[2]) eqs = [connect(world.frame_b, freeMotion.frame_a) connect(freeMotion.frame_b, body.frame_a)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [world; freeMotion; body]) @@ -799,7 +799,7 @@ y = sol(0:0.1:10, idxs = body.r_0[2]) world = Multibody.world @named body = Body(m = 1, isroot = true) -@named model = ODESystem([], t, +@named model = System([], t, systems = [world; body]) # ssys = structural_simplify(model, allow_parameters = false) @@ -830,7 +830,7 @@ world = Multibody.world eqs = [connect(world.frame_b, freeMotion.frame_a) connect(freeMotion.frame_b, body.frame_a)] -@named model = ODESystem(eqs, t, +@named model = System(eqs, t, systems = [world; freeMotion; body]) @@ -919,7 +919,7 @@ number_of_links = 6 connections = [connect(world.frame_b, rope.frame_a) connect(rope.frame_b, body.frame_a)] -@named stiff_rope = ODESystem(connections, t, systems = [world, body, rope]) +@named stiff_rope = System(connections, t, systems = [world, body, rope]) # ssys = structural_simplify(model, allow_parameter = false) @time "Simplify stiff rope pendulum" ssys = structural_simplify(multibody(stiff_rope)) @@ -949,7 +949,7 @@ number_of_links = 3 connections = [connect(world.frame_b, rope.frame_a) connect(rope.frame_b, body.frame_a)] -@named flexible_rope = ODESystem(connections, t, systems = [world, body, rope]) +@named flexible_rope = System(connections, t, systems = [world, body, rope]) # ssys = structural_simplify(model, allow_parameter = false) @time "Simplify flexible rope pendulum" ssys = structural_simplify(multibody(flexible_rope)) @@ -1003,7 +1003,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(damper.flange_b, joint2.axis) ] -@named model = ODESystem(connections, t, systems = [world; systems]) +@named model = System(connections, t, systems = [world; systems]) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ @@ -1036,7 +1036,7 @@ end connections = [connect(world.frame_b, fixed.frame_a, chain.frame_a) connect(chain.frame_b, fixed.frame_b)] - @named mounted_chain = ODESystem(connections, t, systems = [systems; world]) + @named mounted_chain = System(connections, t, systems = [systems; world]) ssys = structural_simplify(multibody(mounted_chain)) prob = ODEProblem(ssys, [ @@ -1159,7 +1159,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, rod.frame_a) connect(rod.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [world, joint, body, rod]) irsys = multibody(model) ssys = structural_simplify(irsys) @@ -1180,7 +1180,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, rod.frame_a) connect(rod.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [world, joint, body, rod]) irsys = multibody(model) ssys = structural_simplify(irsys) @@ -1201,7 +1201,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, rod.frame_a) connect(rod.frame_b, body.frame_a)] -@named model = ODESystem(connections, t, +@named model = System(connections, t, systems = [world, joint, body, rod]) irsys = multibody(model) ssys = structural_simplify(irsys) diff --git a/test/test_fourbar.jl b/test/test_fourbar.jl index c4c929ff..9591814c 100644 --- a/test/test_fourbar.jl +++ b/test/test_fourbar.jl @@ -37,7 +37,7 @@ connections = [connect(j2.frame_b, b2.frame_a) connect(b0.frame_a, world.frame_b) connect(b0.frame_b, j2.frame_a) ] -@named fourbar2 = ODESystem(connections, t, systems = [world; systems]) +@named fourbar2 = System(connections, t, systems = [world; systems]) fourbar2 = complete(fourbar2) ssys = structural_simplify(multibody(fourbar2)) @@ -66,7 +66,7 @@ connections = [connect(j2.frame_b, b2.frame_a) connect(b3.frame_b, j2.frame_a) ] -@named model = ODESystem(connections, t, systems = [world; systems]) +@named model = System(connections, t, systems = [world; systems]) model = complete(model) ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0.0, 1.4399)) # The end time is chosen to make the animation below appear to loop forever From c4891014c9a9aa42b461e54906fba991558aca34 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 26 Aug 2025 09:16:51 +0200 Subject: [PATCH 06/16] remove more defaults --- docs/src/examples/spherical_pendulum.md | 3 +-- src/joints.jl | 2 +- src/orientation.jl | 4 ++-- src/robot/path_planning.jl | 7 +++---- src/robot/robot_components.jl | 2 +- 5 files changed, 8 insertions(+), 10 deletions(-) diff --git a/docs/src/examples/spherical_pendulum.md b/docs/src/examples/spherical_pendulum.md index 6990909c..e717cf26 100644 --- a/docs/src/examples/spherical_pendulum.md +++ b/docs/src/examples/spherical_pendulum.md @@ -9,7 +9,6 @@ This example models a spherical pendulum. The pivot point is modeled using a [`S using Multibody using ModelingToolkit using Plots -# using JuliaSimCompiler using OrdinaryDiffEq t = Multibody.t @@ -29,7 +28,7 @@ connections = [connect(world.frame_b, joint.frame_a) model = complete(model) ssys = structural_simplify(multibody(model)) -prob = ODEProblem(ssys, [], (0, 5)) +prob = ODEProblem(ssys, [], (0, 5), guesses=unknowns(ssys).=>0.1) sol = solve(prob, Rodas4()) @assert SciMLBase.successful_retcode(sol) diff --git a/src/joints.jl b/src/joints.jl index e48a92d9..6841bc8f 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -205,7 +205,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin # @parameters begin # Currently not using parameters due to these appearing in if statements # sequence[1:3] = sequence # end - @variables begin (w_rel(t)[1:3] = zeros(3)), + @variables begin (w_rel(t)[1:3] = state ? zeros(3) : nothing), [ description = "relative angular velocity of frame_b with respect to frame_a, resolved in frame_b", ] end diff --git a/src/orientation.jl b/src/orientation.jl index e46bad2b..dcadecdc 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -41,12 +41,12 @@ Never call this function directly from a component constructor, instead call `f """ function NumRotationMatrix(; R = collect(1.0I(3)), w = zeros(3), name=:R, varw = false, state_priority=nothing) # The reason for not calling this directly is that all R vaiables have to have the same name since they are treated as connector variables (otherwise a connection error is thrown). A component with more than one rotation matrix will thus have two different R variables that overwrite each other - R = at_variables_t(:R, 1:3, 1:3; default = R, state_priority) #[description="Orientation rotation matrix ∈ SO(3)"] + R = at_variables_t(:R, 1:3, 1:3)#; default = R, state_priority) #[description="Orientation rotation matrix ∈ SO(3)"] # @variables w(t)[1:3]=w [description="angular velocity"] # R = collect(R) # R = ModelingToolkit.renamespace.(name, R) .|> Num if varw - w = at_variables_t(:w, 1:3, default = w) + w = at_variables_t(:w, 1:3)#, default = w) else w = get_w(R) end diff --git a/src/robot/path_planning.jl b/src/robot/path_planning.jl index 01f34fde..38e8475e 100644 --- a/src/robot/path_planning.jl +++ b/src/robot/path_planning.jl @@ -185,9 +185,9 @@ function KinematicPTP(; time, name, q0 = 0, q1 = 1, qd_max=1, qdd_max=1) q_vec, qd_vec, qdd_vec = point_to_point(time; q0 = q0, q1 = q1, qd_max, qdd_max) interp_eqs = map(1:nout) do i - qfun = CubicSpline(q_vec[:, i], time; extrapolate=true) - qdfun = LinearInterpolation(qd_vec[:, i], time; extrapolate=true) - qddfun = ConstantInterpolation(qdd_vec[:, i], time; extrapolate=true) + qfun = CubicSpline(q_vec[:, i], time; extrapolation=ExtrapolationType.Constant) + qdfun = LinearInterpolation(qd_vec[:, i], time; extrapolation=ExtrapolationType.Constant) + qddfun = ConstantInterpolation(qdd_vec[:, i], time; extrapolation=ExtrapolationType.Constant) [q.u[i] ~ qfun(t) qd.u[i] ~ qdfun(t) qdd.u[i] ~ qddfun(t)] @@ -196,7 +196,6 @@ function KinematicPTP(; time, name, q0 = 0, q1 = 1, qd_max=1, qdd_max=1) System(eqs, t; name, systems) end - """ Kinematic5(; time, name, q0 = 0, q1 = 1, qd0 = 0, qd1 = 0, qdd0 = 0, qdd1 = 0) diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index ed9d728d..12814fe4 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -66,7 +66,7 @@ Ideal rotational sensor to measure the absolute flange angular acceleration a.u ~ D(w) flange.tau ~ 0 ] - return System(eqs, t, [], []; name = name, systems = [flange, a]) + return System(eqs, t; name = name, systems = [flange, a]) end RotationalFlange = Rotational.Flange From bfc879d23cdbb8fbc9e8c7dd2ef71251cdd3cada Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 17 Sep 2025 11:09:42 +0200 Subject: [PATCH 07/16] fix some rendering issues --- ext/Render.jl | 4 ++-- src/sensors.jl | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index df4ff473..8c208ec2 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -107,7 +107,7 @@ function get_all_vars(model, sol, vars = Multibody.collect_all(unknowns(model))) catch true end - for sys in model.systems + for sys in getfield(model, :systems) if ModelingToolkit.isframe(sys) dorender || continue newvars = Multibody.ModelingToolkit.renamespace.(getfield(model, :name), Multibody.Symbolics.unwrap.(vec(ori(sys).R))) @@ -430,7 +430,7 @@ end Internal function: Recursively render all subsystem components of a multibody system. If a particular component returns `true` from its `render!` method, indicating that the component performaed rendering, the recursion stops. """ function recursive_render!(scene, model, sol, t) - for subsys in model.systems + for subsys in getfield(model, :systems) system_type = get_systemtype(subsys) # did_render = render!(scene, system_type, subsys, sol, t) subsys_ns = getproperty(model, getfield(subsys, :name)) diff --git a/src/sensors.jl b/src/sensors.jl index 34906215..a09f875a 100644 --- a/src/sensors.jl +++ b/src/sensors.jl @@ -29,7 +29,7 @@ function BasicRelativePosition(; name, resolve_frame) error("resolve_frame must be :world, :frame_a or :frame_b, you provided $resolve_frame, which makes me sad.") end - extend(compose(System(eqs, t; name), r_rel), prb) + extend(compose(System(collect(eqs), t; name), r_rel), prb) end function RelativePosition(; name, resolve_frame = :frame_a) From a2c83c94a27fc863e1a3c9b08c4af4619cb762cd Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 17 Sep 2025 11:58:15 +0200 Subject: [PATCH 08/16] complete now does flattening which broke rendering --- ext/Render.jl | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index 8c208ec2..53dd4b19 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -302,7 +302,7 @@ function render(model, sol, size = (600,450), kwargs... ) - ModelingToolkit.iscomplete(model) || (model = complete(model)) + ModelingToolkit.iscomplete(model) || (model = complete(model, flatten=false)) if sol isa ODEProblem sol = FakeSol(model, sol) return render(model, sol, 0; x, y, z, lookat, up, show_axis, kwargs...)[1] @@ -316,7 +316,7 @@ function render(model, sol, t = Observable(timevec[1]) - recursive_render!(scene, complete(model), sol, t) + recursive_render!(scene, model, sol, t) if traces !== nothing tvec = range(sol.t[1], stop=sol.t[end], length=500) @@ -365,11 +365,12 @@ function render(model, sol, time::Real; z = 2, cache = true, size = (1200,1000), + slider = !(sol isa Union{ODEProblem, FakeSol}), kwargs..., ) - ModelingToolkit.iscomplete(model) || (model = complete(model)) - slider = !(sol isa Union{ODEProblem, FakeSol}) + ModelingToolkit.iscomplete(model) || (model = complete(model, flatten=false)) + # model = ModelingToolkit.toggle_namespacing(model, false) if sol isa ODEProblem sol = FakeSol(model, sol) @@ -391,7 +392,7 @@ function render(model, sol, time::Real; else t = Observable(time) end - recursive_render!(scene, complete(model), sol, t) + recursive_render!(scene, model, sol, t) if traces !== nothing tvec = range(sol.t[1], stop=sol.t[end], length=500) @@ -864,7 +865,7 @@ function render!(scene, ::Function, sys, sol, t, args...) # Fallback for systems sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true catch end - frameinds = findall(ModelingToolkit.isframe, collect(sys.systems)) + frameinds = findall(ModelingToolkit.isframe, collect(getfield(sys, :systems))) length(frameinds) == 2 || return false nameof(sys.systems[frameinds[1]]) ∈ (:frame_a, :frame_b) || return false From bcdb24a1e40cf0c93e4e74837886a9853c5c2506 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 18 Sep 2025 08:24:58 +0200 Subject: [PATCH 09/16] use connect in body instead of parameter forwarding --- src/PlanarMechanics/components.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index f0afa2ff..587e18c8 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -130,7 +130,7 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames @components begin translation = FixedTranslation(; r) translation_cm = FixedTranslation(; r=r_cm) - body = Body(; r=r_cm, I, m, gy) + body = Body(; I, m, gy) frame_a = Frame() frame_b = Frame() end From 542d0af282d524194197bee1982bf9f0d437d858 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 18 Sep 2025 11:15:03 +0200 Subject: [PATCH 10/16] some rendering fixes --- ext/Render.jl | 11 +++++++++-- src/PlanarMechanics/components.jl | 4 ++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index 53dd4b19..1b794fcc 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -430,14 +430,21 @@ end """ Internal function: Recursively render all subsystem components of a multibody system. If a particular component returns `true` from its `render!` method, indicating that the component performaed rendering, the recursion stops. """ -function recursive_render!(scene, model, sol, t) +function recursive_render!(scene, model, sol, t, first=true) + # Render the model itself + if first + did_render = render!(scene, get_systemtype(model), model, sol, t) + if did_render === true + return + end + end for subsys in getfield(model, :systems) system_type = get_systemtype(subsys) # did_render = render!(scene, system_type, subsys, sol, t) subsys_ns = getproperty(model, getfield(subsys, :name)) did_render = render!(scene, system_type, subsys_ns, sol, t) if !something(did_render, false) - recursive_render!(scene, subsys_ns, sol, t) + recursive_render!(scene, subsys_ns, sol, t, false) end end end diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 587e18c8..5f673517 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -187,8 +187,8 @@ A fixed translation between two components (rigid rod) phi ~ frame_a.phi w ~ D(phi) # rigidly connect positions - frame_a.x + r0[1] ~ frame_b.x - frame_a.y + r0[2] ~ frame_b.y + [frame_a.x + frame_a.y] + r0 .~ [frame_b.x, frame_b.y] frame_a.phi ~ frame_b.phi # balancing force including lever principle frame_a.fx + frame_b.fx ~ 0 From b5dedb7e6fb631953818626e5603d550ca8e5c62 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 18 Sep 2025 14:37:26 +0200 Subject: [PATCH 11/16] add fixed rotation component --- src/PlanarMechanics/components.jl | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 5f673517..6d13627b 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -197,6 +197,29 @@ A fixed translation between two components (rigid rod) end end +@mtkmodel FixedRotation begin + @extend frame_a, frame_b = partial_frames = PartialTwoFrames() + + @parameters begin + alpha = 0, [description = "Fixed rotation angle between frame_a and frame_b"] + render = true, [description = "Render the rotation in animations"] + end + + @equations begin + # Positions are the same + frame_a.x ~ frame_b.x + frame_a.y ~ frame_b.y + + # Fixed rotation offset + frame_a.phi + alpha ~ frame_b.phi + + # Force balance + frame_a.fx + frame_b.fx ~ 0 + frame_a.fy + frame_b.fy ~ 0 + frame_a.tau + frame_b.tau ~ 0 + end +end + """ Spring(; name, c_x = 1, c_y = 1, c_phi = 1e5, s_relx0 = 0, s_rely0 = 0, phi_rel0 = 0, s_small = 1.e-10) From d4fb8fb795d7579e34dc6d7032703baeb7193685 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 22 Sep 2025 13:31:22 +0200 Subject: [PATCH 12/16] up compat Meshio --- Project.toml | 2 +- ext/Render.jl | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/Project.toml b/Project.toml index f609f958..27523716 100644 --- a/Project.toml +++ b/Project.toml @@ -34,7 +34,7 @@ Graphs = "1.0" JuliaFormatter = "1.0" LightXML = "0.9" LinearAlgebra = "1.6" -MeshIO = "0.4" +MeshIO = "0.4, 0.5" MetaGraphsNext = "0.7" ModelingToolkit = "10" ModelingToolkitStandardLibrary = "2.7.2" diff --git a/ext/Render.jl b/ext/Render.jl index 1b794fcc..ca596c41 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -11,6 +11,9 @@ export render, loop_render using MeshIO, FileIO using StaticArrays + +import Multibody: get_rot_fun, get_fun, get_frame_fun, get_color, get_shapefile, get_shape + """ This struct is used to mimic a solution object such that a model can be rendered without having an actual solution """ @@ -323,9 +326,9 @@ function render(model, sol, for frame in traces md = get_metadata(frame) (md !== nothing) || error("Only frames can be traced in animations.") - if get(md, :frame, false) + if get(md, Multibody.ModelingToolkit.IsFrame, false) points = get_trans(sol, frame, tvec) |> Matrix - elseif get(md, :frame_2d, false) + elseif get(md, Multibody.PlanarMechanics.IsFrame2D, false) points = get_trans_2d(sol, frame, tvec) |> Matrix else error("Got fishy frame metadata") From ab71019940ce2bbb1c168a41560009fcdab5abc7 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 22 Sep 2025 13:31:51 +0200 Subject: [PATCH 13/16] add orientation helpers --- src/Multibody.jl | 9 +++++++++ src/PlanarMechanics/utils.jl | 12 ++++++++++-- src/orientation.jl | 19 ++++++++++++++++++- 3 files changed, 37 insertions(+), 3 deletions(-) diff --git a/src/Multibody.jl b/src/Multibody.jl index 1746f181..2360ba14 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -355,6 +355,15 @@ include("robot/FullRobot.jl") export PlanarMechanics include("PlanarMechanics/PlanarMechanics.jl") + +# These are extended in the render module +function get_rot_fun end +function get_fun end +function get_frame_fun end +function get_color end +function get_shapefile end +function get_shape end + export SphereVisualizer, CylinderVisualizer, BoxVisualizer include("visualizers.jl") diff --git a/src/PlanarMechanics/utils.jl b/src/PlanarMechanics/utils.jl index 2065ad61..a1dcd165 100644 --- a/src/PlanarMechanics/utils.jl +++ b/src/PlanarMechanics/utils.jl @@ -37,8 +37,16 @@ All variables are resolved in the planar world frame. - `radius`: [m] Radius of each axis in animations """ Frame -function ori_2d(frame) - phi = frame.phi +ori_2d(frame::System) = ori_2d(frame.phi) + + +""" + ori_2d(frame) + ori_2d(phi) + +2D orientation matrix from angle phi (in radians). This is the inverse of [`get_rot`](@ref). +""" +function ori_2d(phi) return [cos(phi) -sin(phi); sin(phi) cos(phi)] end diff --git a/src/orientation.jl b/src/orientation.jl index dcadecdc..98ae4578 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -373,6 +373,16 @@ function resolve_dyade2(R, D1) R*D1*R' end +function is_frame(x) + md = get_metadata(frame) + get(md, Multibody.ModelingToolkit.IsFrame, false) +end + +function is_frame_2d(x) + md = get_metadata(frame) + get(md, Multibody.PlanarMechanics.IsFrame2D, false) +end + """ R_W_F = get_rot(sol, frame, t) @@ -389,7 +399,14 @@ The columns of ``R_W_F`` indicate are the basis vectors of the frame ``F`` expre See also [`get_trans`](@ref), [`get_frame`](@ref), [Orientations and directions](@ref) (docs section). """ function get_rot(sol, frame, t) - Rotations.RotMatrix3(reshape(sol(t, idxs = vec(ori(frame).R.mat')), 3, 3)) + if is_frame(frame) + Rotations.RotMatrix3(reshape(sol(t, idxs = vec(ori(frame).R.mat')), 3, 3)) + elseif is_frame_2d(frame) + phi = sol(t, idxs = frame.phi) + Rotations.RotMatrix2(ori_2d(phi)') + else + error("$frame is not a multibody frame") + end end """ From 30470b3828a4b235ae37013259b06f65c817e62d Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 25 Sep 2025 10:38:09 +0200 Subject: [PATCH 14/16] test tweaks --- src/PlanarMechanics/components.jl | 3 ++- test/test_PlanarMechanics.jl | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index 6d13627b..0dcabdbb 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -116,6 +116,7 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames @structural_parameters begin r = [1,0] r_cm = 0.5*r + state_priority = 2 end @parameters begin gy = -9.80665 @@ -130,7 +131,7 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames @components begin translation = FixedTranslation(; r) translation_cm = FixedTranslation(; r=r_cm) - body = Body(; I, m, gy) + body = Body(; I, m, gy, state_priority) frame_a = Frame() frame_b = Frame() end diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 082b71ad..49a40755 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -3,6 +3,7 @@ using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkit: t_nounits as t, D_nounits as D import ModelingToolkitStandardLibrary.Blocks +import Multibody import Multibody: IRSystem import Multibody.PlanarMechanics as Pl # using JuliaSimCompiler From 0d65c570764ab5a5142d686dad20e26ff32cf873 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 26 Sep 2025 15:51:47 +0200 Subject: [PATCH 15/16] return also scene --- ext/Render.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ext/Render.jl b/ext/Render.jl index ca596c41..fc77358a 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -412,7 +412,7 @@ function render(model, sol, time::Real; Makie.lines!(scene, points) end end - fig, t + fig, t, scene end function Multibody.loop_render(model, sol; timescale = 1.0, framerate = 30, max_loop = 5, kwargs...) From 78d4fbba2e40d542e51edcfeb9d81e15766931a5 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Mon, 29 Sep 2025 10:04:31 +0200 Subject: [PATCH 16/16] enhance error reporting for frame metadata in render function --- ext/Render.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index fc77358a..06af116c 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -331,7 +331,7 @@ function render(model, sol, elseif get(md, Multibody.PlanarMechanics.IsFrame2D, false) points = get_trans_2d(sol, frame, tvec) |> Matrix else - error("Got fishy frame metadata") + error("Got fishy frame metadata", md) end Makie.lines!(scene, points) end @@ -402,12 +402,12 @@ function render(model, sol, time::Real; for frame in traces md = get_metadata(frame) (md !== nothing) || error("Only frames can be traced in animations.") - if get(md, :frame, false) + if get(md, Multibody.ModelingToolkit.IsFrame, false) points = get_trans(sol, frame, tvec) |> Matrix - elseif get(md, :frame_2d, false) + elseif get(md, Multibody.PlanarMechanics.IsFrame2D, false) points = get_trans_2d(sol, frame, tvec) |> Matrix else - error("Got fishy frame metadata") + error("Got fishy frame metadata", md) end Makie.lines!(scene, points) end