Port Craig–Bampton models to the SciML 3 stack

.github/workflows/Documentation.yml

@@ -30,8 +30,8 @@ jobs:
           version: '1'
       - name: Install dependencies
         run: DISPLAY=:0 xvfb-run --auto-servernum -s '-screen 0 1024x768x24' julia --project=docs -e 'using Pkg;
-                                      Pkg.add(PackageSpec(name = "ModelingToolkitStandardLibrary", url = "https://github.com/SciML/ModelingToolkitStandardLibrary.jl", rev = "main"));
                                       Pkg.develop(PackageSpec(path=pwd()));
+                                      Pkg.add(PackageSpec(name = "ModelingToolkitStandardLibrary", url = "https://github.com/SciML/ModelingToolkitStandardLibrary.jl", rev = "main"));
                                       Pkg.instantiate()'
       - name: Build and deploy
         env:

Project.toml

@@ -1,6 +1,6 @@
 name = "Multibody"
 uuid = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9"
-version = "0.3.2"
+version = "0.4.0"
 authors = ["Fredrik Bagge Carlson, JuliaHub Inc."]
 
 [deps]
@@ -32,20 +32,22 @@ Render = ["Makie"]
 URDF = ["LightXML", "Graphs", "MetaGraphsNext", "JuliaFormatter"]
 
 [compat]
+ADTypes = "1"
 CoordinateTransformations = "0.6"
-DataInterpolations = "8.6.1"
+DataInterpolations = "10"
 FileIO = "1"
 Graphs = "1.0"
 JuliaFormatter = "1.0, 2.0"
 LightXML = "0.9"
 LinearAlgebra = "1.6"
 MeshIO = "0.4, 0.5"
 MetaGraphsNext = "0.7"
-ModelingToolkit = "11.5.1"
+ModelingToolkit = "11.40"
 ModelingToolkitStandardLibrary = "2.26"
-OrdinaryDiffEqNonlinearSolve = "1.14.1"
-OrdinaryDiffEqRosenbrock = "1.17.0"
-OrdinaryDiffEqTsit5 = "1.5.0"
+OrdinaryDiffEqNonlinearSolve = "2"
+OrdinaryDiffEqRosenbrock = "2"
+OrdinaryDiffEqTsit5 = "2"
+Random = "1"
 Rotations = "1.4"
 SparseArrays = "1"
 StateSelection = "1.1.1"
@@ -54,12 +56,14 @@ TrajectoryLimiters = "0.1"
 julia = "1"
 
 [extras]
+ADTypes = "47edcb42-4c32-4615-8424-f2b9edc5f35b"
 Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
 JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899"
 LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179"
 MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
+Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["OrdinaryDiffEq", "Test", "JuliaFormatter", "LightXML", "Graphs", "MetaGraphsNext"]
+test = ["Random", "ADTypes", "OrdinaryDiffEq", "Test", "JuliaFormatter", "LightXML", "Graphs", "MetaGraphsNext"]

docs/Project.toml

@@ -1,4 +1,7 @@
 [deps]
+TrajectoryLimiters = "9792e600-fe43-4e4e-833b-462f466b8006"
+ADTypes = "47edcb42-4c32-4615-8424-f2b9edc5f35b"
+OrdinaryDiffEqRosenbrock = "43230ef6-c299-4910-a778-202eb28ce4ce"
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
 Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"

docs/src/components.md

@@ -6,6 +6,11 @@ A mass with a shape can be modeled using a [`BodyShape`](@ref). The primary diff
 
 A rod without a mass (just a translation), is modeled using [`FixedTranslation`](@ref).
 
+[`CraigBampton`](@ref) models a flexible body from reduced mass and stiffness matrices.
+Its first boundary is the reference frame; additional boundaries expose connector frames
+whose deformation is represented by the reduced model. A single-boundary model can
+represent internal modes, such as fluid sloshing, coupled to the reference frame.
+
 ```@index
 Pages   = ["components.md"]
 ```

docs/src/examples/free_motion.md

@@ -70,10 +70,10 @@ eqs = [connect(bar2.frame_a, world.frame_b)
                          ])
 ssys = multibody(model)
 prob = ODEProblem(ssys, [
-    body.body.v_0 .=> zeros(3);
-    body.body.w_a .=> zeros(3);
+    collect(body.body.v_0) .=> zeros(3);
+    collect(body.body.w_a) .=> zeros(3);
     # body.body.phi .=> deg2rad.([10,10,10]); # If using Euler/Cardan angles
-    body.body.Q̂ .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+    collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
 ], (0, 4))
 
 sol = solve(prob, Rodas5P())

docs/src/examples/kinematic_loops.md

@@ -28,6 +28,7 @@ using ModelingToolkit
 import ModelingToolkitStandardLibrary.Mechanical.Rotational
 using Plots
 using OrdinaryDiffEq
+using ADTypes
 using LinearAlgebra
 # using JuliaSimCompiler
 
@@ -76,8 +77,8 @@ connections = [
 ]
 @named fourbar = System(connections, t; systems)
 ssys = multibody(fourbar)
-prob = ODEProblem(ssys, [fourbar.j1.phi => 0.1], (0.0, 10.0))
-sol = solve(prob, FBDF(autodiff=true))
+prob = ODEProblem(ssys, [ssys.j1.phi => 0.1], (0.0, 10.0))
+sol = solve(prob, FBDF(autodiff=AutoForwardDiff()))
 
 plot(
     plot(sol, idxs = [j1.phi, j2.phi, j3.phi]),
@@ -118,7 +119,7 @@ To drive the mechanism, we set the initial velocity of the joint j1 to some non-
 
 ```@example kinloop
 systems = @named begin
-    j1 = Revolute(n = [1, 0, 0], w = 5.235987755982989, state_priority=10.0, radius=0.1f0) # Increase state priority to ensure that this joint coordinate is chosen as state variable
+    j1 = Revolute(n = [1, 0, 0], w = 5.235987755982989, state_priority=10, radius=0.1f0) # Increase state priority to ensure that this joint coordinate is chosen as state variable
     j2 = Prismatic(n = [1, 0, 0], s = -0.2)
     b1 = BodyShape(r = [0, 0.5, 0.1], radius=0.03)
     b2 = BodyShape(r = [0, 0.2, 0], radius=0.03)
@@ -149,7 +150,7 @@ ssys = multibody(fourbar2)
 
 prob = ODEProblem(ssys, [], (0.0, 1.4399)) # The end time is chosen to make the animation below appear to loop forever
 
-sol = solve(prob, FBDF(autodiff=true));
+sol = solve(prob, FBDF(autodiff=AutoForwardDiff()));
 @test SciMLBase.successful_retcode(sol)
 plot(sol, idxs=[j2.s]) # Plot the joint coordinate of the prismatic joint (green in the animation below)
 ```
@@ -167,7 +168,7 @@ This example models a mechanism similar to the previous one, but replaces severa
 
 ```@example kinloop
 systems = @named begin
-    j1 = Revolute(n = [1, 0, 0], w = 5.235987755983, state_priority=12.0, radius=0.1f0) # Increase state priority to ensure that this joint coordinate is chosen as state variable
+    j1 = Revolute(n = [1, 0, 0], w = 5.235987755983, state_priority=12, radius=0.1f0) # Increase state priority to ensure that this joint coordinate is chosen as state variable
     j2 = Prismatic(n = [1, 0, 0], s = -0.2)
     b1 = BodyShape(r = [0, 0.5, 0.1], radius=0.03)
     b2 = BodyShape(r = [0, 0.2, 0], radius=0.03)
@@ -187,7 +188,7 @@ connections = [connect(j2.frame_b, b2.frame_a)
 @named fourbar_analytic = System(connections, t, systems = [world; systems])
 ssys_analytic = multibody(fourbar_analytic)
 prob = ODEProblem(ssys_analytic, [], (0.0, 1.4399)) 
-sol2 = solve(prob, FBDF(autodiff=true)) # about 4x faster than the simulation above
+sol2 = solve(prob, FBDF(autodiff=AutoForwardDiff())) # about 4x faster than the simulation above
 plot!(sol2, idxs=[j2.s]) # Plot the same coordinate as above
 ```
 In practice, the two simulations are not exactly identical since we haven't modeled any mass attached to the rod in the joint assembly. We could add such mass to the rod by attaching to the `joint_us.frame_ia` connector.
@@ -213,6 +214,6 @@ variables.
 We can also inspect the mass matrices of the two systems to see how many nonlinear algebraic equations the solver has to deal with
 ```@example kinloop
 using LinearAlgebra
-diag(ssys.mass_matrix), diag(ssys_analytic.mass_matrix)
+calculate_massmatrix(ssys), calculate_massmatrix(ssys_analytic)
 ```
-A 1 on the diagonal indicates a differential equation, while a 0 indicates an algebraic equation. The cut-joint version has 6 nonlinear algebraic equations, while the joint assembly version has only 1. Both of them have 2 differential equations (position and velocity), corresponding to the 1 degree of freedom in the mechanism. Nonlinear algebraic equations are more expensive to solve than differential equations.
+A 1 on the diagonal indicates a differential equation, while a 0 indicates an algebraic equation. The cut-joint version has 6 nonlinear algebraic equations, while the joint assembly version has only 1. Both of them have 2 differential equations (position and velocity), corresponding to the 1 degree of freedom in the mechanism. Nonlinear algebraic equations are more expensive to solve than differential equations.

docs/src/examples/pendulum.md

@@ -4,8 +4,10 @@ This beginners tutorial will start by modeling a pendulum pivoted around the ori
 To start, we load the required packages
 ```@example pendulum
 using ModelingToolkit
+import ModelingToolkit: connect
 using Multibody
 using OrdinaryDiffEq # Contains the ODE solver we will use
+using OrdinaryDiffEqRosenbrock
 using Plots
 ```
 We then access the world frame and time variable from the Multibody module
@@ -340,6 +342,10 @@ gray = [0.5, 0.5, 0.5, 1]
         motor = TranslationalModelica.Force(use_support = false)
         tip = Body(m = 0.05)
     end
+    if use_world
+        @named world = World()
+        push!(systems, world)
+    end
 
     vars = @variables begin
         u(t)
@@ -402,15 +408,19 @@ We start by linearizing the model in the upward equilibrium position using the f
 ```@example pendulum
 import ModelingToolkit: D_nounits as D
 using LinearAlgebra
-@named cp = Cartpole()
+@named cp = Cartpole(use_world = true)
 cp = complete(cp)
 inputs = [cp.u] # Input to the linearized system
 outputs = [cp.x, cp.phi, cp.v, cp.w] # These are the outputs of the linearized system
-op = Dict([ # Operating point to linearize in
+op = merge(initial_conditions(cp), Dict([ # Operating point to linearize in
     cp.u => 0
+    cp.prismatic.s => 0
+    cp.prismatic.v => 0
+    D(cp.prismatic.s) => 0
+    cp.revolute.w => 0
+    D(cp.revolute.phi) => 0
     cp.revolute.phi => 0 # Pendulum pointing upwards
-]
-)
+]))
 matrices, simplified_sys = linearize(cp, inputs, outputs; op, Multibody.linsys...)
 matrices
 ```
@@ -494,8 +504,7 @@ nothing # hide
 Below, we add also an energy-based swing-up controller. For more details this kind of swing-up controller, see [Part 7: Control of rotary pendulum using Julia: Swing up control (YouTube)](https://www.youtube.com/watch?v=RhF2NMCYoiw)
 ```@example pendulum
 "Compute total energy, kinetic + potential, for a body rotating around the z-axis of the world"
-function energy(body, w)
-    g = GlobalScope(world.g_inner)
+function energy(body, w, g)
     m = body.m
     d2 = body.r_cm[1]^2 + body.r_cm[2]^2 # Squared distance from 
     I = body.I_33 + m*d2 # Parallel axis theorem
@@ -530,7 +539,7 @@ normalize_angle(x::Number) = mod(x+3.1415, 2pi)-3.1415
     equations = Equation[
         phi ~ normalize_angle(cartpole.phi)
         w ~ cartpole.w
-        E ~ energy(cartpole.pendulum.body, w) + energy(cartpole.tip, w)
+        E ~ energy(cartpole.pendulum.body, w, 9.80665) + energy(cartpole.tip, w, 9.80665)
         u_swing ~ 100*(E - Er)*sign(w*cos(phi-3.1415))
 
         L.input.u[1] ~ 0 - namespaced_outputs[1] # - cartpole.x

docs/src/examples/prescribed_pose.md

@@ -24,6 +24,7 @@ using Multibody.Rotations # To specify orientations using Euler angles
 using ModelingToolkit
 using Plots
 using OrdinaryDiffEq
+using ADTypes
 using LinearAlgebra
 # using JuliaSimCompiler
 using Test
@@ -163,7 +164,7 @@ defs = [
     ssys.excited_suspension.suspension.r2.phi => -0.6031*(1)
 ]
 prob = ODEProblem(ssys, defs, (0, 2π))
-sol = solve(prob, Rodas5P(autodiff=false)) 
+sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff()))
 @test SciMLBase.successful_retcode(sol)
 Multibody.render(model, sol, show_axis=false, x=-0.8, y=0.7, z=0.1, lookat=[0,0.1,0.0], filename="prescribed_motion.gif") # Video
 nothing # hide

docs/src/examples/quad.md

@@ -9,16 +9,18 @@ The main body of the aircraft is modeled using a [`Body`](@ref), and the arms ar
 ```@example QUAD
 using Multibody
 using ModelingToolkit
+import ModelingToolkit: connect
 using ModelingToolkitStandardLibrary.Blocks
 using LinearAlgebra
 using Plots
 # using JuliaSimCompiler
 using OrdinaryDiffEq
+using ADTypes
 using Test
 t = Multibody.t
 D = Differential(t)
 
-world = Multibody.world
+world = World(; name = :world, g = 9.81)
 
 num_arms = 4 # Number of arms of the rotor craft.
 angle_between_arms = 2*pi/num_arms # Angle between the arms, assuming they are evenly spaced.
@@ -199,7 +201,7 @@ op = [
 ]
 
 @time "ODEProblem" prob = ODEProblem(ssys, op, (0, 20))
-sol = solve(prob, FBDF(autodiff=false), reltol=1e-8, abstol=1e-8)
+sol = solve(prob, FBDF(autodiff=AutoFiniteDiff()), reltol=1e-8, abstol=1e-8)
 @test SciMLBase.successful_retcode(sol)
 
 plot(sol, idxs=[ssys.arm1.frame_b.r_0[2], ssys.arm2.frame_b.r_0[2], ssys.arm3.frame_b.r_0[2], ssys.arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines)
@@ -229,7 +231,6 @@ outputs = [quad.y_alt, quad.y_roll, quad.y_pitch, quad.y_yaw, quad.y_forward, qu
 op = [
     quad.body.r_0[2] => 1e-32
     quad.v_alt => 1e-32 # To avoid singularity in linearization
-    quad.world.g => 9.81
     inputs .=> 1;
 ] |> Dict
 
@@ -261,7 +262,6 @@ L
 
 
 ```@example QUAD
-ModelingToolkit.get_iv(i::IRSystem) = i.t
 model = RotorCraft(; closed_loop=true, addload=true, L=-L, outputs) # Negate L for negative feedback
 ssys = multibody(model)
 
@@ -270,14 +270,13 @@ op = [
     ssys.body.r_0[3] => 1e-3
     ssys.body.r_0[1] => 1e-3
     ssys.cable.joint_2.phi .=> 1 # Usa a larger initial cable bend since this controller is more robust
-    ssys.world.g => 9.81;
     ssys.body.phid .=> 0;
     D(ssys.body.phi) .=> 0;
     ssys.feedback_gain.input.u .=> 0;
     ssys.Ie_alt => -10; # Initialize the integrator state to avoid a very large initial transient. This pre-compensates for gravity
 ] |> Dict
 prob = ODEProblem(ssys, op, (0, 20))
-sol = solve(prob, FBDF(autodiff=false))
+sol = solve(prob, FBDF(autodiff=AutoFiniteDiff()))
 @test SciMLBase.successful_retcode(sol)
 plot(sol, idxs=[ssys.arm1.frame_b.r_0[2], ssys.arm2.frame_b.r_0[2], ssys.arm3.frame_b.r_0[2], ssys.arm4.frame_b.r_0[2]], layout=4, framestyle=:zerolines)
 ```
@@ -298,20 +297,20 @@ linop = merge(op, Dict([
     ssys.system_outputs.u .=> 1e-32
     ssys.feedback_gain.input.u .=> 1e-32
     ]))
-@time "Sensitivity function" S = get_named_sensitivity(model, :y; system_modifier=IRSystem, op=linop)
+@time "Sensitivity function" S = get_named_sensitivity(model, :y; system_modifier=multibody, op=linop)
 S = minreal(S, 1e-6)
 isstable(S) || @error "Sensitivity function S is not stable"
 T = I(S.ny) - S
 T = minreal(T, 1e-6)
 isstable(T) || @error "Sensitivity function T is not stable"
-LT = feedback(T, -I(T.ny))#get_named_looptransfer(model, :y; system_modifier=IRSystem, op)
+LT = feedback(T, -I(T.ny))#get_named_looptransfer(model, :y; system_modifier=multibody, op)
 
-@time "Comp Sensitivity function" Ti = get_named_comp_sensitivity(model, :u; system_modifier=IRSystem, op=linop)
+@time "Comp Sensitivity function" Ti = get_named_comp_sensitivity(model, :u; system_modifier=multibody, op=linop)
 Ti = minreal(Ti, 1e-6)
 isstable(Ti) || @error "Sensitivity function Ti is not stable"
 LTi = feedback(Ti, -I(Ti.ny)) # Input loop-transfer function
 
-CS = named_ss(model, :y, :u; op=linop, system_modifier=IRSystem) # Closed-loop system from measurement noise to control signal
+CS = named_ss(model, :y, :u; op=linop, system_modifier=multibody) # Closed-loop system from measurement noise to control signal
 
 w = 2pi.*exp10.(LinRange(-2, 2, 200))
 fig_dm = plot(diskmargin(LT, 0.5), label="Plant output") # Compute diskmargin with a positive skew of 0.5 to account for a likely gain increase when the load is dropped

docs/src/examples/robot.md

@@ -8,6 +8,7 @@ using ModelingToolkit
 using Plots
 # using JuliaSimCompiler
 using OrdinaryDiffEq
+using ADTypes
 using Test
 
 t = Multibody.t
@@ -26,18 +27,22 @@ unknowns(ssys)
 
 ```@example robot
 prob = ODEProblem(ssys, Dict([
-    robot.mechanics.r1.phi => deg2rad(-60)
-    robot.mechanics.r2.phi => deg2rad(20)
-    robot.mechanics.r3.phi => deg2rad(90)
-    robot.mechanics.r4.phi => deg2rad(0)
-    robot.mechanics.r5.phi => deg2rad(-110)
-    robot.mechanics.r6.phi => deg2rad(0)
-
-    robot.axis1.motor.Jmotor.phi => deg2rad(-60) *  -105 # Multiply by gear ratio
-    robot.axis2.motor.Jmotor.phi => deg2rad(20) *  210
-    robot.axis3.motor.Jmotor.phi => deg2rad(90) *  60
+    ssys.mechanics.r1.phi => deg2rad(-60)
+    ssys.mechanics.r2.phi => deg2rad(20)
+    ssys.mechanics.r3.phi => deg2rad(90)
+    ssys.mechanics.r4.phi => deg2rad(0)
+    ssys.mechanics.r5.phi => deg2rad(-110)
+    ssys.mechanics.r6.phi => deg2rad(0)
+
+    ssys.axis1.motor.Jmotor.phi => deg2rad(-60) *  -105 # Multiply by gear ratio
+    ssys.axis2.motor.Jmotor.phi => deg2rad(20) *  210
+    ssys.axis3.motor.Jmotor.phi => deg2rad(90) *  60
+
+    ssys.axis1.motor.Jmotor.w => 0
+    ssys.axis2.motor.Jmotor.w => 0
+    ssys.axis3.motor.Jmotor.w => 0
 ]), (0.0, 2.0))
-sol = solve(prob, Rodas5P(autodiff=false));
+sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff()));
 @test SciMLBase.successful_retcode(sol)
 
 plot(sol, idxs = [