diff --git a/.github/workflows/Documentation.yml b/.github/workflows/Documentation.yml index ce448d00..5cf638a4 100644 --- a/.github/workflows/Documentation.yml +++ b/.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: diff --git a/Project.toml b/Project.toml index 757adbbc..ab0d3063 100644 --- a/Project.toml +++ b/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,8 +32,9 @@ 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" @@ -41,11 +42,12 @@ 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"] diff --git a/docs/Project.toml b/docs/Project.toml index 543b7d0d..bfb66cb4 100644 --- a/docs/Project.toml +++ b/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" diff --git a/docs/src/components.md b/docs/src/components.md index 96f2b1a0..83cb47de 100644 --- a/docs/src/components.md +++ b/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"] ``` diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index 53bc3fbf..603f0f70 100644 --- a/docs/src/examples/free_motion.md +++ b/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()) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index a78f49e3..7f87e06d 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/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. \ No newline at end of file +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. diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index b68a2d3f..f358bf6f 100644 --- a/docs/src/examples/pendulum.md +++ b/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 diff --git a/docs/src/examples/prescribed_pose.md b/docs/src/examples/prescribed_pose.md index e5c58c64..52fa554d 100644 --- a/docs/src/examples/prescribed_pose.md +++ b/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 diff --git a/docs/src/examples/quad.md b/docs/src/examples/quad.md index 66e12b20..116fed4c 100644 --- a/docs/src/examples/quad.md +++ b/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 diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 54f6b707..9ddabfd3 100644 --- a/docs/src/examples/robot.md +++ b/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 = [ diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index ffa33a0e..c2523f52 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -17,6 +17,8 @@ using ModelingToolkit using Plots # using JuliaSimCompiler using OrdinaryDiffEq +using OrdinaryDiffEqRosenbrock +using ADTypes using Test t = Multibody.t @@ -33,7 +35,7 @@ connections = [connect(world.frame_b, rope.frame_a) stiff_rope = complete(stiff_rope) ssys = multibody(stiff_rope) prob = ODEProblem(ssys, [], (0, 5)) -sol = solve(prob, Rodas4(autodiff=false)) +sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff())) @test SciMLBase.successful_retcode(sol) import GLMakie @@ -57,7 +59,7 @@ connections = [connect(world.frame_b, rope.frame_a) flexible_rope = complete(flexible_rope) ssys = multibody(flexible_rope) prob = ODEProblem(ssys, [], (0, 8)) -sol = solve(prob, Rodas4(autodiff=false)); +sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff())); @test SciMLBase.successful_retcode(sol) Multibody.render(flexible_rope, sol, y = -3, x = -6, z = -6, lookat=[0, -3, 0], filename = "flexible_rope.gif") # May take long time for n>=10 @@ -93,7 +95,7 @@ prob = ODEProblem(ssys, [ chain.link_8.body.w_a .=> [0,0,0]; chain.link_8.frame_b.r_0 .=> [x_dist,0,0]; ], (0, 4)) -sol = solve(prob, Rodas4(autodiff=false)) +sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff())) @test SciMLBase.successful_retcode(sol) Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10 diff --git a/docs/src/examples/sensors.md b/docs/src/examples/sensors.md index ee1a5bee..65241c98 100644 --- a/docs/src/examples/sensors.md +++ b/docs/src/examples/sensors.md @@ -9,6 +9,7 @@ using ModelingToolkit # using JuliaSimCompiler using Plots using OrdinaryDiffEq +using OrdinaryDiffEqRosenbrock using LinearAlgebra t = Multibody.t @@ -38,6 +39,7 @@ D = Differential(t) prob = ODEProblem(ssys, [], (0, 3)) using OrdinaryDiffEq +using OrdinaryDiffEqRosenbrock sol = solve(prob, Rodas4()) @assert SciMLBase.successful_retcode(sol) diff --git a/docs/src/examples/suspension.md b/docs/src/examples/suspension.md index 565ac500..df3dc5bb 100644 --- a/docs/src/examples/suspension.md +++ b/docs/src/examples/suspension.md @@ -12,6 +12,7 @@ using ModelingToolkit import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational using Plots using OrdinaryDiffEq +using ADTypes using LinearAlgebra # using JuliaSimCompiler using Test @@ -172,7 +173,7 @@ defs = [ display(sort(unknowns(ssys), by=string)) prob = ODEProblem(ssys, defs, (0, 2)) -sol = solve(prob, FBDF(autodiff=true)) +sol = solve(prob, FBDF(autodiff=AutoForwardDiff())) @test SciMLBase.successful_retcode(sol) rms(x) = sqrt(sum(abs2, x) / length(x)) @test rms(sol(0:0.1:2, idxs=ssys.body_upright.v)) ≈ 2.740 atol=0.01 @@ -261,7 +262,7 @@ defs = [ display(sort(unknowns(ssys), by=string)) prob = ODEProblem(ssys, defs, (0, 4)) -sol = solve(prob, FBDF(autodiff=true), initializealg = ShampineCollocationInit()) +sol = solve(prob, FBDF(autodiff=AutoForwardDiff()), initializealg = ShampineCollocationInit()) @test SciMLBase.successful_retcode(sol) ``` @@ -367,7 +368,7 @@ defs = [ ssys.body_upright.v => 0.14 ] prob = ODEProblem(ssys, defs, (0, 4)) -sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit()) # FBDF is inefficient for models including the `SlippingWheel` component due to the discontinuous second-order derivative of the slip model +sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff()), initializealg = BrownFullBasicInit()) # FBDF is inefficient for models including the `SlippingWheel` component due to the discontinuous second-order derivative of the slip model @assert all(sol[ssys.excited_suspension.wheel.wheeljoint.f_n] .> 0) "Model not valid for negative normal forces" @test SciMLBase.successful_retcode(sol) Multibody.render(model, sol, show_axis=false, x=-1.3, y=0.3, z=0.0, lookat=[0,0.1,0.0], filename="suspension_wheel.gif") # Video @@ -441,7 +442,7 @@ defs = [ display(sort(unknowns(ssys), by=string)) prob = ODEProblem(ssys, defs, (0, 4)) -sol = solve(prob, Rodas5P(autodiff=false), initializealg = ShampineCollocationInit()) # FBDF is inefficient for models including the `SlippingWheel` component due to the discontinuous second-order derivative of the slip model +sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff()), initializealg = ShampineCollocationInit()) # FBDF is inefficient for models including the `SlippingWheel` component due to the discontinuous second-order derivative of the slip model @test SciMLBase.successful_retcode(sol) @assert all(sol[ssys.excited_suspension_r.wheel.wheeljoint.f_n] .> 0) "Model not valid for negative normal forces" @assert all(sol[ssys.excited_suspension_l.wheel.wheeljoint.f_n] .> 0) "Model not valid for negative normal forces" @@ -538,7 +539,7 @@ defs = [ display(sort(unknowns(ssys), by=string)) prob = ODEProblem(ssys, defs, (0, 3)) -sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit()) +sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff()), initializealg = BrownFullBasicInit()) @test SciMLBase.successful_retcode(sol) @assert all(reduce(hcat, sol[[ssys.excited_suspension_bl.wheel.wheeljoint.f_n, ssys.excited_suspension_br.wheel.wheeljoint.f_n, ssys.excited_suspension_fl.wheel.wheeljoint.f_n, ssys.excited_suspension_fr.wheel.wheeljoint.f_n]]) .> 0) "Model not valid for negative normal forces" diff --git a/docs/src/examples/swing.md b/docs/src/examples/swing.md index c5c8318f..806e029c 100644 --- a/docs/src/examples/swing.md +++ b/docs/src/examples/swing.md @@ -13,17 +13,18 @@ using ModelingToolkit using Plots # using JuliaSimCompiler using OrdinaryDiffEq +using ADTypes t = Multibody.t D = Differential(t) world = Multibody.world -@component function SwingRope(; name) +@component function SwingRope(; name, r = [0.0, -1, 0]) systems = @named begin frame_a = Frame() frame_b = Frame() joint1 = Spherical(isroot=true, state=true, d=0.001, color=[0.7, 0.7, 0.7, 0.7]) - rope = BodyShape(r=[0.0,-1,0], m=0.05, radius=0.01) + rope = BodyShape(; r, m=0.05, radius=0.01) spring = Spring(c = inv(0.04/60), m=0.01, radius=0.06) damper = Damper(d = 50.0, radius=0.05, length_fraction=0.1) end @@ -46,7 +47,7 @@ end systems = @named begin world = World() upper_trans1 = FixedTranslation(r=[-w/2, 0, 0]) - rope1 = SwingRope(rope.r=[-w/2, h, -w/2]) + rope1 = SwingRope(r=[-w/2, h, -w/2]) body = Body(m=6, isroot=true, I_11=0.1, I_22=0.1, I_33=0.1) damper = Damper(d=10.0) end @@ -70,7 +71,7 @@ prob = ODEProblem(ssys, [ model.body.v_0 .=> 0; model.body.w_a .=> 0; ], (0, 4)) -sol = solve(prob, ImplicitEuler(autodiff=false), reltol=5e-3) +sol = solve(prob, ImplicitEuler(autodiff=AutoFiniteDiff()), reltol=5e-3) @assert SciMLBase.successful_retcode(sol) ``` @@ -96,10 +97,10 @@ Next, we create the full swing assembly world = World() upper_trans1 = FixedTranslation(r=[-w/2, 0, 0]) upper_trans2 = FixedTranslation(r=[ w/2, 0, 0]) - rope1 = SwingRope(rope.r=[-w/2, -h, -w/2]) - rope2 = SwingRope(rope.r=[-w/2, -h, w/2]) - rope3 = SwingRope(rope.r=[ w/2, -h, -w/2]) - rope4 = SwingRope(rope.r=[ w/2, -h, w/2]) + rope1 = SwingRope(r=[-w/2, -h, -w/2]) + rope2 = SwingRope(r=[-w/2, -h, w/2]) + rope3 = SwingRope(r=[ w/2, -h, -w/2]) + rope4 = SwingRope(r=[ w/2, -h, w/2]) body_back = BodyShape(m=0.1, r = [w, 0, 0]) body_front = BodyShape(m=0.1, r = [w, 0, 0]) @@ -159,7 +160,7 @@ prob = ODEProblem(ssys, [ ], (0.0, 6)) -@time sol = solve(prob, ImplicitEuler(autodiff=false), reltol=1e-2) +@time sol = solve(prob, ImplicitEuler(autodiff=AutoFiniteDiff()), reltol=1e-2) @assert SciMLBase.successful_retcode(sol) Plots.plot(sol, idxs = model.body.r_0) diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index e25ddd7a..12970bef 100644 --- a/docs/src/examples/wheel.md +++ b/docs/src/examples/wheel.md @@ -25,6 +25,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational import ModelingToolkitStandardLibrary.Blocks using Plots using OrdinaryDiffEq +using ADTypes using LinearAlgebra using Random using Test @@ -269,7 +270,7 @@ tire_black = [0.1, 0.1, 0.1, 1] connect(prefer_straight_ahead.flange_b, steering_joint.support) ] - return System(equations, t; name, systems) + return System(equations, t, vars, pars; name, systems) end @named model = Car() ssys = multibody(model) @@ -343,6 +344,7 @@ import ModelingToolkitStandardLibrary.Blocks import Multibody.PlanarMechanics as Pl using Plots using OrdinaryDiffEq +using ADTypes using LinearAlgebra # using JuliaSimCompiler using Test @@ -395,7 +397,6 @@ end @named model = TestSlipBasedWheel() ssys = multibody(model) -defs = ModelingToolkit.defaults(model) prob = ODEProblem(ssys, [ ssys.inertia.w => 1e-10, # This is important, at zero velocity, the friction is ill-defined ssys.revolute.frame_b.phi => 0, @@ -533,11 +534,12 @@ end ssys = multibody(model) defs = merge( Dict(unknowns(ssys) .=> 0), - ModelingToolkit.defaults(model), + ModelingToolkit.initial_conditions(model), + ModelingToolkit.bindings(model), Dict(ssys.body.w => 0), ) prob = ODEProblem(ssys, defs, (0.0, 5.0)) -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, y=0, z=5, filename="twotrack.gif", cache=false) nothing # hide diff --git a/docs/src/joints.md b/docs/src/joints.md index 8cc93ab3..83acc90c 100644 --- a/docs/src/joints.md +++ b/docs/src/joints.md @@ -2,7 +2,7 @@ A joint restricts the number of degrees of freedom (DOF) of a body. For example, a free floating body has 6 DOF, but if it is attached to a [`Revolute`](@ref) joint, the joint restricts all but one rotational degree of freedom (a revolute joint acts like a hinge). Similarily, a [`Prismatic`](@ref) joint restricts all but one translational degree of freedom (a prismatic joint acts like a slider). -A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. It thus transmits no torque. A [`Planar`](@ref) joint moves in a plane, i.e., it restricts one translational DOF and two rotational DOF. A [`Universal`](@ref) joint has two rotational DOF. +A [`Spherical`](@ref) joints restricts all translational degrees of freedom, but allows all rotational degrees of freedom. With `d=0` it transmits no torque; positive damping opposes relative angular velocity. A [`Planar`](@ref) joint moves in a plane, i.e., it restricts one translational DOF and two rotational DOF. A [`Universal`](@ref) joint has two rotational DOF. Some joints offer the option to add 1-dimensional components to them by providing the keyword `axisflange = true`. This allows us to add, e.g., springs, dampers, sensors, and actuators to the joint. diff --git a/examples/sloshing_tank_cart.jl b/examples/sloshing_tank_cart.jl new file mode 100644 index 00000000..5895a472 --- /dev/null +++ b/examples/sloshing_tank_cart.jl @@ -0,0 +1,251 @@ +#= +# Sloshing Water Tank on a Cart + +This example demonstrates the CraigBampton component by modeling a cart on a +horizontal prismatic joint with a water tank on top. The water sloshing dynamics +are modeled using Craig-Bampton modal reduction. + +## Physical Setup + +``` + ┌─────────────────┐ + │ Water Tank │ ← Sloshing water (CB model) + │ ~~~~~~~~~~~~ │ + │ │ + └────────┬────────┘ + │ + ┌────────┴────────┐ + │ Cart │ ← Rigid body + └────────┬────────┘ + │ + ═══════════════════ ← Prismatic joint (horizontal) + World +``` + +## Sloshing Physics + +For a rectangular tank with length L and water depth h, the sloshing frequency is: + ω_n = sqrt(g * π * n / L * tanh(π * n * h / L)) + +The participating (effective sloshing) mass for mode n is: + m_n = 8 * ρ * L * W * h / (π³ * n³) * tanh(π * n * h / L) +=# + +using Multibody +using ModelingToolkit +using OrdinaryDiffEq +using LinearAlgebra +using Plots + +t = Multibody.t +D = Differential(t) +world = Multibody.world + +# ============================================================================= +# Tank and fluid parameters +# ============================================================================= + +# Tank geometry +L_tank = 1.0 # Tank length in x-direction (m) +W_tank = 0.5 # Tank width in y-direction (m) +h_water = 0.4 # Water depth (m) +h_tank = 0.5 # Tank height (m) +t_wall = 0.01 # Wall thickness (m) + +# Material properties +ρ_water = 1000.0 # Water density (kg/m³) +ρ_tank = 2700.0 # Tank material density (aluminum, kg/m³) +g = 9.81 # Gravitational acceleration (m/s²) + +# Cart parameters +m_cart = 50.0 # Cart mass (kg) +cart_length = 0.4 # Cart length (m) + +# Driving force (square wave) +F_amp = 100.0 # Force amplitude (N) +F_freq = 0.03 # Frequency (Hz) + +# Damping +ζ_slosh = 0.02 # Sloshing damping ratio (2%) + +# ============================================================================= +# Compute sloshing model parameters +# ============================================================================= + +# Total water mass +m_water = ρ_water * L_tank * W_tank * h_water +println("Water mass: $(m_water) kg") + +# Approximate tank structure mass (simplified: just the walls) +m_tank_structure = ρ_tank * t_wall * ( + 2 * L_tank * h_tank + # Front and back walls + 2 * W_tank * h_tank + # Side walls + L_tank * W_tank # Bottom +) * W_tank # This is approximate +m_tank_structure = 20.0 # Simplified: assume 20 kg tank structure + +# Sloshing frequencies for first two modes +function sloshing_frequency(n, L, h, g) + sqrt(g * π * n / L * tanh(π * n * h / L)) +end + +ω_1 = sloshing_frequency(1, L_tank, h_water, g) +ω_2 = sloshing_frequency(2, L_tank, h_water, g) +println("Sloshing frequencies: ω₁ = $(round(ω_1, digits=2)) rad/s, ω₂ = $(round(ω_2, digits=2)) rad/s") +println("Sloshing periods: T₁ = $(round(2π/ω_1, digits=2)) s, T₂ = $(round(2π/ω_2, digits=2)) s") + +# Participating mass for each mode (mass that effectively sloshes) +function participating_mass(n, ρ, L, W, h) + 8 * ρ * L * W * h / (π^3 * n^3) * tanh(π * n * h / L) +end + +m_1 = participating_mass(1, ρ_water, L_tank, W_tank, h_water) +m_2 = participating_mass(2, ρ_water, L_tank, W_tank, h_water) +println("Participating masses: m₁ = $(round(m_1, digits=2)) kg, m₂ = $(round(m_2, digits=2)) kg") +println("Total participating: $(round(m_1 + m_2, digits=2)) kg ($(round(100*(m_1+m_2)/m_water, digits=1))% of water)") + +# ============================================================================= +# Build Craig-Bampton matrices for sloshing model +# ============================================================================= + +n_modes = 2 +n_boundaries = 2 # Tank bottom (attached) and top (free) +n_bdof = 6 * n_boundaries + +# Total mass at boundary (tank + water) +m_total = m_tank_structure + m_water + +# Approximate moments of inertia for the tank+water assembly +I_xx = m_total * (W_tank^2 + h_tank^2) / 12 +I_yy = m_total * (L_tank^2 + h_tank^2) / 12 +I_zz = m_total * (L_tank^2 + W_tank^2) / 12 + +# M_BB: Boundary mass matrix +# For boundary 1 (bottom, attached to cart): full mass +# For boundary 2 (top, free): small mass (just for numerical stability) +M_BB = zeros(n_bdof, n_bdof) +# Boundary 1 (indices 1-6): main mass +M_BB[1,1] = m_total # x translation +M_BB[2,2] = m_total # y translation +M_BB[3,3] = m_total # z translation +M_BB[4,4] = I_xx # rotation about x +M_BB[5,5] = I_yy # rotation about y +M_BB[6,6] = I_zz # rotation about z +# Boundary 2 (indices 7-12): small mass for free end +M_BB[7,7] = 0.1 +M_BB[8,8] = 0.1 +M_BB[9,9] = 0.1 +M_BB[10,10] = 0.01 +M_BB[11,11] = 0.01 +M_BB[12,12] = 0.01 + +# M_BI: Boundary-modal coupling +# For mass-normalized modes (M_II = I), the coupling is sqrt(participating_mass) +# Physical: m_n * η̈_n + k_n * η_n = -m_n * ẍ +# Mass-normalized (ξ = η*√m): ξ̈ + ω²ξ = -√m_n * ẍ +M_BI = zeros(n_bdof, n_modes) +M_BI[1, 1] = sqrt(m_1) # x-acceleration couples to mode 1 +M_BI[1, 2] = sqrt(m_2) # x-acceleration couples to mode 2 + +# M_II: Modal mass matrix (identity for mass-normalized modes) +M_II = Matrix(1.0I, n_modes, n_modes) + +# K_BB: Boundary stiffness (zero for rigid connection) +K_BB = zeros(n_bdof, n_bdof) +# Add small stiffness for the free boundary to prevent drift +K_BB[7,7] = 100.0 +K_BB[8,8] = 100.0 +K_BB[9,9] = 100.0 +K_BB[10,10] = 10.0 +K_BB[11,11] = 10.0 +K_BB[12,12] = 10.0 + +# K_II: Modal stiffness matrix (diagonal with ω²) +ω_modes = [ω_1, ω_2] +K_II = Diagonal(ω_modes.^2) + +# Damping ratios +ζ_modes = [ζ_slosh, ζ_slosh] + +# ============================================================================= +# Create the multibody model +# ============================================================================= + +systems = @named begin + # Prismatic joint for cart horizontal motion + prismatic = Prismatic(n=[1, 0, 0], axisflange=true) + + # Cart body + cart = BodyShape(m=m_cart, r=[cart_length, 0, 0], radius=0.05, color=[0.3, 0.3, 0.3, 1]) + + # Mounting point on top of cart for tank + mount = FixedTranslation(r=[cart_length/2, h_tank/2, 0]) + + # Sloshing water tank (Craig-Bampton model) + tank = CraigBampton( + boundary_positions = [[0.0, 0.0, 0.0], [0.0, h_tank, 0.0]], # Bottom and top + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ_modes, + ω = ω_modes, + ) + + # Damper on prismatic joint for cart friction + cart_damper = Multibody.Translational.Damper(d=250.0) + + # Square wave driving force + force_signal = Multibody.Blocks.Square(frequency=F_freq, amplitude=F_amp) + drive_force = Multibody.Translational.Force() +end + +# Connect the system +eqs = [ + connect(world.frame_b, prismatic.frame_a) + connect(prismatic.frame_b, cart.frame_a) + connect(cart.frame_b, mount.frame_a) + connect(mount.frame_b, tank.frame_a) + connect(prismatic.axis, cart_damper.flange_a) + connect(prismatic.support, cart_damper.flange_b) + connect(force_signal.output, drive_force.f) + connect(drive_force.flange, prismatic.axis) +] + +@named model = System(eqs, t; systems=[world; systems]) + +# ============================================================================= +# Simulate: Square wave driven cart +# ============================================================================= + +println("\n=== Building and simulating model ===") +ssys = multibody(model) + +prob = ODEProblem(ssys, [ + prismatic.v => 0.0, + prismatic.s => 0.0, +], (0.0, 50.0)) + +sol = solve(prob, Tsit5()) +println("Simulation complete: $(sol.retcode)") + +# ============================================================================= +# Plot results +# ============================================================================= + +# Cart position and velocity +p1 = plot(sol, idxs=[prismatic.s], label="Cart position (m)", ylabel="Position") +p2 = plot(sol, idxs=[prismatic.v], label="Cart velocity (m/s)", ylabel="Velocity") + +# Sloshing mode amplitudes +p3 = plot(sol, idxs=[tank.η[1]], label="Mode 1 (ω=$(round(ω_1,digits=1)) rad/s)", ylabel="η") +plot!(p3, sol, idxs=[tank.η[2]], label="Mode 2 (ω=$(round(ω_2,digits=1)) rad/s)") + +# Driving force +p4 = plot(sol, idxs=[force_signal.output.u], label="Drive force (N)", ylabel="Force") + +# Combined plot +p = plot(p1, p2, p3, p4, layout=(4,1), size=(800, 800), + title=["Square-Wave Driven Cart with Sloshing Tank" "" "" ""]) +display(p) diff --git a/reproducers/unused_array_initialization.jl b/reproducers/unused_array_initialization.jl new file mode 100644 index 00000000..a40227c5 --- /dev/null +++ b/reproducers/unused_array_initialization.jl @@ -0,0 +1,10 @@ +using ModelingToolkit +using InteractiveUtils +using ModelingToolkit: t_nounits as t, D_nounits as D + +versioninfo() +@variables x(t)=0.0 u(t)[1:3] +@named unused = System(Equation[], t, [u], []) +@named sys = System([D(x) ~ 1.0], t; systems = [unused]) +compiled = mtkcompile(sys) +ODEProblem(compiled, [], (0.0, 1.0)) diff --git a/src/Multibody.jl b/src/Multibody.jl index d9fa4aaa..cb6f399c 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -350,7 +350,7 @@ include("frames.jl") export PartialTwoFrames include("interfaces.jl") -export World, world, Mounting1D, Fixed, Position, Pose, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope +export World, world, Mounting1D, Fixed, Position, Pose, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope, CraigBampton include("components.jl") export Revolute, Prismatic, Planar, Spherical, Universal, diff --git a/src/PlanarMechanics/components.jl b/src/PlanarMechanics/components.jl index a88b748e..e10a5aa2 100644 --- a/src/PlanarMechanics/components.jl +++ b/src/PlanarMechanics/components.jl @@ -46,6 +46,7 @@ Body component with mass and inertia - `radius`: [m] Radius of the body in animations - `render`: [Bool] Render the body in animations - `color`: [Array{Float64,1}] Color of the body in animations +- `state_priority = 0`: Favors joint coordinates over dependent body coordinates # Variables: - `r`: [m, m] x,y position @@ -58,7 +59,7 @@ Body component with mass and inertia # Connectors: - `frame`: 2-dim. Coordinate system """ -@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) +@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=0) systems = @named begin frame_a = Frame() end @@ -108,6 +109,7 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames # Parameters - `r`: (Structural) Vector from `frame_a` to `frame_b` resolved in `frame_a` - `r_cm`: (Structural) Vector from `frame_a` to the center of mass resolved in `frame_a` +- `state_priority = 0`: State selection priority passed to the internal `Body` # Subsystems - `translation`: [FixedTranslation](@ref) Fixed translation between `frame_a` and `frame_b` @@ -118,7 +120,7 @@ The `BodyShape` component is similar to a [`Body`](@ref), but it has two frames - `frame_a` - `frame_b` """ -@component function BodyShape(; name, r0=nothing, r = [1, 0], r_cm = 0.5*r, state_priority = 2, +@component function BodyShape(; name, r0=nothing, r = [1, 0], r_cm = 0.5*r, state_priority = 0, gy = -9.80665, m = 1, I = 0.1, radius = 0.1, render = true, color = purple, v=nothing, phi=nothing, w=nothing) pars = @parameters begin @@ -682,7 +684,7 @@ The force depends with friction characteristics on the slip. The slip is split i - lateral slip: the lateral velocity divided by the rolling velocity. - longitudinal slip: the longitudinal slip velocity divided by the rolling velocity. -For low rolling velocity this definition become ill-conditioned. Hence a dry-friction model is used for low rolling velocities. For **zero rolling velocity**, the intitialization might fail if automatic differentiation is used. Either start with a non-zero (but tiny) rolling velocity or pass `autodiff=false` to the solver. +For low rolling velocity this definition become ill-conditioned. Hence a dry-friction model is used for low rolling velocities. For **zero rolling velocity**, the intitialization might fail if automatic differentiation is used. Either start with a non-zero (but tiny) rolling velocity or pass `autodiff=AutoFiniteDiff()` to the solver. The radius of the wheel can be specified by the parameter `radius`. The driving direction (for `phi = 0`) can be specified by the parameter `r`. The normal load is set by `N`. diff --git a/src/components.jl b/src/components.jl index 63277585..c89d8309 100644 --- a/src/components.jl +++ b/src/components.jl @@ -81,7 +81,7 @@ If a connection to the world is needed in a component model, use [`Fixed`](@ref) end """ -The world component is the root of all multibody models. It is a fixed frame with a parallel gravitational field and a gravity vector specified by the unit direction `world.n` (defaults to [0, -1, 0]) and magnitude `world.g` (defaults to 9.80665). +The world component is the root of all multibody models. It is a fixed frame with a parallel gravitational field. The `n` and `g` arguments of [`World`](@ref) specify the unit direction and magnitude of gravity. """ const world = World(; name = :world) @@ -191,7 +191,7 @@ See also [`Position`](@ref) for a component that allows for only forced translat if R !== nothing ori(frame_b).R ~ R elseif q !== nothing - ori(frame_b) ~ from_Q(q, 0; normalize) + ori(frame_b) ~ from_Q(q, zeros(3); normalize) else error("Either R or q must be provided. If you only want to specify the position, use the `Position` component instead.") end @@ -521,11 +521,11 @@ See also [`BodyCylinder`](@ref) and [`BodyBox`](@ref) for body components with p state_priority = 2, description = "Position vector from origin of world frame to origin of frame_a", ] - v_0(t)[1:3], [ guess=0, + v_0(t)[1:3], [ guess=zeros(3), state_priority = 2, description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))", ] - a_0(t)[1:3], [ guess=0, + a_0(t)[1:3], [ guess=zeros(3), description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))", ] end @@ -863,3 +863,341 @@ Rigid body with box shape. The mass properties of the body (mass, center of mass return System(equations, t, vars, pars; name, systems) end + +""" + CraigBampton(; name, boundary_positions, M_BB, M_BI, M_II, K_BB, K_II, ζ, ω) + +A flexible body component using Craig-Bampton reduced-order modeling. This allows +including reduced flexible body models derived from external FEM tools. + +The Craig-Bampton method reduces a FEM model by partitioning degrees of freedom into: +- **Boundary DOFs (q_B)**: Interface nodes that connect to other multibody components +- **Internal DOFs**: Reduced to modal coordinates η + +The reduced equations of motion are: +``` +[M_BB M_BI] [q̈_B] [C_BB 0 ] [q̇_B] [K_BB 0 ] [q_B] [f_B] +[M_IB M_II] [η̈ ] + [0 C_II] [η̇ ] + [0 K_II] [η ] = [f_I] +``` + +where `C_II = diag(2*ζ_i*ω_i*m_ii)` provides modal damping. + +# Arguments +- `boundary_positions`: Vector of 3-element vectors, positions of boundary nodes in body reference frame. + The first boundary corresponds to `frame_a`, additional boundaries create `frame_b`, `frame_c`, etc. +- `M_BB`: Boundary mass matrix `[6*n_boundaries × 6*n_boundaries]` +- `M_BI`: Boundary-internal coupling mass matrix `[6*n_boundaries × n_modes]` +- `M_II`: Internal/modal mass matrix `[n_modes × n_modes]` (often diagonal for mass-normalized modes) +- `K_BB`: Boundary stiffness matrix `[6*n_boundaries × 6*n_boundaries]` +- `K_II`: Internal/modal stiffness matrix `[n_modes × n_modes]` (diagonal = ω² for mass-normalized modes) +- `ζ`: Vector of modal damping ratios `[n_modes]` +- `ω`: Vector of modal frequencies in rad/s `[n_modes]`, used to compute damping matrix + +Each boundary has 6 DOFs: 3 translations + 3 rotations (small angle approximation). + +# Connectors +- `frame_a`: Reference boundary frame (first boundary position) +- `frame_b`, `frame_c`, ...: Additional boundary frames + +# State Variables +- `η[1:n_modes]`: Modal amplitudes +- `η̇[1:n_modes]`: Modal velocities +- `q_B[7:6*n_boundaries]`: Boundary deformations for boundaries 2..n (relative to frame_a) +- `q̇_B[7:6*n_boundaries]`: Boundary deformation velocities + +The first boundary (frame_a) serves as the reference with zero deformation. Body motion +comes from how frame_a is connected externally (e.g., to joints or world). + +# Example +```julia +# Simple 2-boundary flexible beam (10 modes) +n_modes = 10 +n_boundaries = 2 +n_bdof = 12 # 6 DOFs per boundary + +# Mass-normalized mode shapes give M_II = I, K_II = diag(ω²) +M_BB = [...] # From FEM export +M_BI = [...] # From FEM export +M_II = diagm(ones(n_modes)) +K_II = diagm(ω.^2) +ζ = 0.02 * ones(n_modes) # 2% damping all modes +ω = [10, 25, 40, ...] # Modal frequencies (rad/s) + +@named flexible_beam = CraigBampton( + boundary_positions = [[0,0,0], [1,0,0]], # Beam ends + M_BB = M_BB, M_BI = M_BI, M_II = M_II, + K_BB = zeros(n_bdof, n_bdof), K_II = K_II, + ζ = ζ, ω = ω +) + +# Connect to world at frame_a, attach payload at frame_b +connections = [ + connect(world.frame_b, flexible_beam.frame_a) + connect(flexible_beam.frame_b, payload.frame_a) +] +``` + +See also [`Body`](@ref), [`BodyShape`](@ref) for rigid body components. +""" +@component function CraigBampton(; + name, + boundary_positions, # Vector of 3-vectors: [[x1,y1,z1], [x2,y2,z2], ...] + M_BB, # Boundary mass matrix [6*n_b × 6*n_b] + M_BI, # Boundary-internal coupling [6*n_b × n_modes] + M_II, # Internal modal mass [n_modes × n_modes] + K_BB, # Boundary stiffness [6*n_b × 6*n_b] + K_II, # Internal modal stiffness [n_modes × n_modes] + ζ, # Modal damping ratios [n_modes] + ω, # Modal frequencies for computing damping [n_modes] + render = true, + color = [0.6, 0.6, 0.8, 1.0], +) + n_boundaries = length(boundary_positions) + n_modes = size(M_II, 1) + n_bdof = 6 * n_boundaries # 6 DOFs per boundary (3 trans + 3 rot) + + @assert size(M_BB) == (n_bdof, n_bdof) "M_BB must be $(n_bdof)×$(n_bdof)" + @assert size(M_BI) == (n_bdof, n_modes) "M_BI must be $(n_bdof)×$(n_modes)" + @assert size(M_II) == (n_modes, n_modes) "M_II must be $(n_modes)×$(n_modes)" + @assert size(K_BB) == (n_bdof, n_bdof) "K_BB must be $(n_bdof)×$(n_bdof)" + @assert size(K_II) == (n_modes, n_modes) "K_II must be $(n_modes)×$(n_modes)" + @assert length(ζ) == n_modes "ζ must have length $(n_modes)" + @assert length(ω) == n_modes "ω must have length $(n_modes)" + + # Compute modal damping matrix C_II = diag(2*ζ*ω*m_ii) + C_II_diag = 2 .* ζ .* ω .* diag(M_II) + + # Create frames - frame_a is reference, then frame_b, frame_c, etc. + @named frame_a = Frame() + frames = Any[frame_a] + if n_boundaries >= 2 + @named frame_b = Frame() + push!(frames, frame_b) + end + if n_boundaries >= 3 + @named frame_c = Frame() + push!(frames, frame_c) + end + if n_boundaries >= 4 + @named frame_d = Frame() + push!(frames, frame_d) + end + if n_boundaries >= 5 + @named frame_e = Frame() + push!(frames, frame_e) + end + if n_boundaries >= 6 + @named frame_f = Frame() + push!(frames, frame_f) + end + if n_boundaries > 6 + error("CraigBampton currently supports at most 6 boundary frames") + end + + # Store matrices as parameters + pars = @parameters begin + render = render, [description = "Render the component in animations"] + color[1:4] = color, [description = "Color in animations (RGBA)"] + end + + # State variables - modal coordinates + @variables begin + η(t)[1:n_modes] = zeros(n_modes), [description = "Modal amplitudes"] + η̇(t)[1:n_modes] = zeros(n_modes), [description = "Modal velocities"] + end + + # Boundary deformation variables for boundaries 2..n (boundary 1 = frame_a is reference) + # Each boundary has 6 DOFs: [ux, uy, uz, θx, θy, θz] + n_flex_dof = 6 * (n_boundaries - 1) # DOFs for flexible deformations (excluding reference) + + @variables begin + q_flex(t)[1:n_flex_dof] = zeros(n_flex_dof), [description = "Boundary deformations (boundaries 2..n)"] + q̇_flex(t)[1:n_flex_dof] = zeros(n_flex_dof), [description = "Boundary deformation velocities"] + end + + # Reference frame acceleration variables (needed for modal coupling) + @variables begin + r_0(t)[1:3], [description = "Position of frame_a origin in world frame"] + v_0(t)[1:3], [description = "Velocity of frame_a origin in world frame"] + a_0(t)[1:3], [description = "Acceleration of frame_a origin in world frame"] + w_a(t)[1:3], [description = "Angular velocity of frame_a in body frame"] + z_a(t)[1:3], [description = "Angular acceleration of frame_a in body frame"] + end + + # Reference frame orientation + R_a = ori(frame_a) + + # Build equation arrays + eqs = Equation[] + + # frame_a kinematics (following Body component pattern) + push!(eqs, r_0 ~ frame_a.r_0) + push!(eqs, v_0 ~ D(r_0)) + push!(eqs, a_0 ~ D(v_0)) + push!(eqs, w_a ~ angular_velocity2(R_a)) + push!(eqs, z_a ~ D(w_a)) + + # Boundary 1 acceleration in body frame (drives modal excitation through M_BI coupling) + # Subtract gravity so that only the "dynamic" acceleration excites modes and contributes + # to inertial forces, matching the Body component convention: a_0 - g_0 + g_0 = gravity_acceleration(frame_a.r_0) + a_body = resolve2(R_a, a_0 - g_0) + q̈_B1 = vcat(collect(a_body), collect(z_a)) + + # Kinematic equations for states + push!(eqs, D(η) ~ η̇) + if n_flex_dof > 0 + push!(eqs, D(q_flex) ~ q̇_flex) + end + + # Build full q_B vector: [zeros(6); q_flex] for stiffness terms (deformations only) + q_B_full = vcat(zeros(Num, 6), collect(q_flex)) + q̇_B_full = vcat(zeros(Num, 6), collect(q̇_flex)) + + # Second derivatives: boundary 1 uses actual frame_a acceleration, rest use deformation accelerations + η_ddot = D.(collect(η̇)) + q̈_B_full = vcat(q̈_B1, D.(collect(q̇_flex))) + + # Modal dynamics equation: + # M_II * η̈ + C_II * η̇ + K_II * η = -M_BI' * q̈_B + # Note: M_IB = M_BI' (symmetric coupling) + modal_forcing = -M_BI' * q̈_B_full + for i in 1:n_modes + lhs = sum(M_II[i,j] * η_ddot[j] for j in 1:n_modes) + + C_II_diag[i] * η̇[i] + + sum(K_II[i,j] * η[j] for j in 1:n_modes) + push!(eqs, lhs ~ modal_forcing[i]) + end + + # Boundary force equations: + # The generalized forces at boundaries from CB dynamics: + # f_B = K_BB * q_B + M_BB * q̈_B + M_BI * η̈ + f_B = K_BB * q_B_full + M_BB * q̈_B_full + M_BI * η_ddot + + # Frame kinematics and force balance for each boundary + for i in 1:n_boundaries + frame_i = frames[i] + p_i = boundary_positions[i] # Undeformed position in body frame + + # Index into q_B for this boundary's DOFs + idx_start = 6*(i-1) + 1 + idx_end = 6*i + + # Forces and torques in body frame + f_i = f_B[idx_start:idx_start+2] + τ_i = f_B[idx_start+3:idx_end] + + if i == 1 + # frame_a is the reference - its position is determined by external connections + # Force balance (in connector/body frame) + append!(eqs, collect(frame_a.f) .~ f_i) + append!(eqs, collect(frame_a.tau) .~ τ_i) + else + # Other boundaries have position determined by deformation + flex_idx = 6*(i-2) + 1 # Index into q_flex + u_i = q_flex[flex_idx:flex_idx+2] # Translation deformation + θ_i = q_flex[flex_idx+3:flex_idx+5] # Rotation deformation (small angles) + + # Position: r_i = r_a + R_a * (p_i + u_i) + r_rel = p_i + collect(u_i) + append!(eqs, collect(frame_i.r_0) .~ collect(frame_a.r_0) + resolve1(R_a, r_rel)) + + # Orientation: same as frame_a (small angle approximation) + append!(eqs, ori(frame_i) ~ R_a) + + # Force balance (in connector/body frame) + append!(eqs, collect(frame_i.f) .~ f_i) + append!(eqs, collect(frame_i.tau) .~ τ_i) + end + end + + System(eqs, t, [η; η̇; q_flex; q̇_flex; r_0; v_0; a_0; w_a; z_a], pars; name, systems=frames) +end + +#= +CB TODO +The "simplified" version has these limitations: + + 1. frame_a is the fixed reference - body motion comes entirely from external connections to frame_a + 2. No rigid body dynamics - the CB component doesn't have its own mass/inertia for overall body motion + 3. No coupling between rigid motion and flexible modes - Coriolis/centrifugal effects are ignored + + To lift these restrictions and create a floating frame of reference implementation: + + Key Changes Needed + + 1. Add Rigid Body States + + Like the Body component with isroot=true: + + @variables begin + # Body reference frame position/velocity in world + r_0(t)[1:3] # Position of body frame origin + v_0(t)[1:3] # Translational velocity + + # Orientation (quaternions or Euler angles) + Q(t)[1:4] # Quaternion orientation + # OR phi(t)[1:3] for Euler angles + + w_a(t)[1:3] # Angular velocity in body frame + end + + 2. Extended Mass Matrix + + The full floating-frame CB equations are: + + [M_rr M_rf M_rB] [q̈_r] [h_r] [f_r] + [M_fr M_ff M_fB] [η̈ ] + [h_f] = [f_f] + [M_Br M_Bf M_BB] [q̈_B] [h_B] [f_B] + + Where: + - q_r = rigid body DOFs (6: 3 translation + 3 rotation) + - M_rr = rigid body inertia (6×6) - total mass and inertia tensor + - M_rf = rigid-flexible coupling - how modes affect rigid body motion + - h = quadratic velocity terms (Coriolis, centrifugal) + + 3. New Input Parameters + + CraigBampton(; + # ... existing parameters ... + + # Floating frame parameters + floating = false, # Enable floating frame + m_total, # Total mass of flexible body + I_cm, # Inertia tensor at center of mass + r_cm, # Position of CM in body frame + M_rf, # Rigid-flexible coupling matrix [6 × n_modes] + + # Optional + isroot = false, # Whether this body provides root states + quat = true, # Use quaternions for orientation + ) + + 4. Frame Kinematics Changes + + Currently frame_a is special (reference). With floating frame, all boundaries are equal: + + # Current (simplified): + frame_i.r_0 = frame_a.r_0 + R_a * (p_i + u_i) + + # Floating frame version: + frame_i.r_0 = r_0 + R_body * (p_i + u_i) # All frames relative to body frame + + 5. Quadratic Velocity Terms + + For rotating bodies, Coriolis and centrifugal forces couple rigid and flexible motion: + + # Coriolis terms for flexible modes + h_f = -2 * M_rf' * [0; 0; 0; cross(w_a, w_a)] # Simplified + + # Centrifugal stiffening (geometric stiffness) + K_geo = f(w_a, stress) # Stress-dependent stiffness + + 6. Gravity Distribution + + Gravity must act on the distributed mass: + + g_0 = gravity_acceleration(r_0 + R_body * r_cm) + f_gravity = m_total * g_0 # Applied at CM + +=# diff --git a/src/fancy_joints.jl b/src/fancy_joints.jl index ef2eabfc..9e493353 100644 --- a/src/fancy_joints.jl +++ b/src/fancy_joints.jl @@ -50,7 +50,7 @@ It is not possible to connect other components, such as a body with mass propert end rodlength = _norm(r_0) - constraint_residue = rRod_0'rRod_0 - rodlength^2 + constraint_residue = sum(abs2, collect(rRod_0)) - rodlength^2 eqs = [ @@ -99,7 +99,7 @@ It is not possible to connect other components, such as a body with mass propert [r_cm_0 ~ frame_a.r_0 + rRod_0/2; v_cm_0 ~ D(r_cm_0); f_cm_a ~ m*resolve2(ori(frame_a), D(v_cm_0) - gravity_acceleration(r_cm_0)) - f_cm_e ~ (f_cm_a'eRod_a)*eRod_a + f_cm_e ~ dot(collect(f_cm_a), collect(eRod_a))*eRod_a frame_a.f ~ (f_cm_a - f_cm_e)./2 + f_rod*eRod_a f_b_a1 ~ (f_cm_a + f_cm_e)./2 frame_b.f ~ resolve_relative(f_b_a1 - f_rod*eRod_a, ori(frame_a), diff --git a/src/forces.jl b/src/forces.jl index cebfc071..01214163 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -135,9 +135,9 @@ end end eqs = [r_0 ~ frame_b.r_0 - frame_a.r_0 - frame_b.tau ~ 0 - 0 ~ frame_a.f + resolve2(frame_a, f_b_0) - 0 ~ frame_a.tau + resolve2(frame_a, cross(r_0, f_b_0))] + frame_b.tau ~ zeros(3) + zeros(3) ~ frame_a.f + resolve2(frame_a, f_b_0) + zeros(3) ~ frame_a.tau + resolve2(frame_a, cross(r_0, f_b_0))] if resolve_frame == :frame_a append!(eqs, diff --git a/src/joints.jl b/src/joints.jl index d14920a0..501fd5e1 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -177,6 +177,11 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin - `radius = 0.1`: Radius of the joint in animations - `color = [1,1,0,1]`: Color of the joint in animations, a vector of length 4 with values between [0, 1] providing RGBA values - `render = true`: Render the joint in animations + +`w_rel` is the relative angular velocity resolved in `frame_b`, including when +`state=false`. For positive `d`, damping opposes this velocity; the reaction torque +is transformed into `frame_a` coordinates. + """ @component function Spherical(; name, state = false, isroot = true, iscut=false, w_rel_a_fixed = false, z_rel_a_fixed = false, sequence = [1, 2, 3], phi = state ? zeros(3) : nothing, @@ -190,11 +195,11 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin ) dnum = d - @named begin - ptf = PartialTwoFrames() - Rrel = NumRotationMatrix() - Rrel_inv = NumRotationMatrix() - end + @named ptf = PartialTwoFrames() + Rrel_matrix = at_variables_t(:Rrel, 1:3, 1:3; state_priority = -1) + Rrel_inv_matrix = at_variables_t(:Rrel_inv, 1:3, 1:3; state_priority = -1) + Rrel = RotationMatrix(Rrel_matrix, get_w(Rrel_matrix)) + Rrel_inv = RotationMatrix(Rrel_inv_matrix, get_w(Rrel_inv_matrix)) pars = @parameters begin radius = radius, [description = "radius of the joint in animations"] color[1:4] = color, [description = "color of the joint in animations (RGBA)"] @@ -210,6 +215,11 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin description = "relative angular velocity of frame_b with respect to frame_a, resolved in frame_b", ] end + if state + vars = [vars; vec(Rrel_matrix)] + isroot || (vars = [vars; vec(Rrel_inv_matrix)]) + end + # torque balance if dnum <= 0 eqs = [zeros(3) ~ frame_a.tau @@ -217,8 +227,8 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin frame_b.r_0 ~ frame_a.r_0] else fric = d*w_rel - eqs = [-fric ~ frame_a.tau - fric ~ resolve1(Rrel, frame_b.tau) + eqs = [-resolve_relative(fric, frame_b, frame_a) ~ frame_a.tau + fric ~ frame_b.tau frame_b.r_0 ~ frame_a.r_0] end @@ -260,16 +270,11 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin else # Spherical joint does not have state - append!(eqs, - #frame_b.r_0 ~ transpose(frame_b.R.T)*(frame_b.R.T*(transpose(frame_a.R.T)*(frame_a.R.T*frame_a.r_0))); + push!(eqs, zeros(3) ~ frame_a.f + resolve_relative(frame_b.f, frame_b, frame_a)) - if w_rel_a_fixed || z_rel_a_fixed - append!(w_rel .~ angular_velocity2(frame_b) - resolve2(frame_b, - angular_velocity1(frame_a))) - else - append!(w_rel .~ zeros(3)) - end + append!(eqs, w_rel .~ angular_velocity2(ori(frame_b)) - resolve2(frame_b, + angular_velocity1(ori(frame_a)))) end extend(System(eqs, t, vars, pars; name), ptf) @@ -304,11 +309,11 @@ Joint where `frame_a` rotates around axis `n_a` which is fixed in `frame_a` and end @unpack frame_a, frame_b = ptf pars = @parameters begin - n_a = n_a, + n_a[1:3] = n_a, [ description = "Axis of revolute joint 1 resolved in frame_a", ] - n_b = n_b, + n_b[1:3] = n_b, [ description = "Axis of revolute joint 2 resolved in frame_b", ] @@ -388,11 +393,11 @@ This ideal massless joint provides a gear constraint between frames `frame_a` an pars = @parameters begin ratio = ratio, [description = "Gear speed ratio"] - n_a = n_a, + n_a[1:3] = n_a, [ description = "Axis of rotation of shaft a (same coordinates in frame_a, frame_b, bearing)", ] - n_b = n_b, + n_b[1:3] = n_b, [ description = "Axis of rotation of shaft b (same coordinates in frame_a, frame_b, bearing)", ] @@ -518,10 +523,10 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi end pars = eltype(vars)[] - @named Rrel_f = Frame() - @named Rrel_inv_f = Frame() - Rrel = ori(Rrel_f) - Rrel_inv = ori(Rrel_inv_f) + Rrel_matrix = at_variables_t(:Rrel, 1:3, 1:3; state_priority = -1) + Rrel_inv_matrix = at_variables_t(:Rrel_inv, 1:3, 1:3; state_priority = -1) + Rrel = RotationMatrix(Rrel_matrix, get_w(Rrel_matrix)) + Rrel_inv = RotationMatrix(Rrel_inv_matrix, get_w(Rrel_inv_matrix)) eqs = [ # Cut-forces and cut-torques are zero @@ -536,10 +541,12 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi frame_b.r_0 ~ frame_a.r_0 + resolve1(frame_a, r_rel_a)] if state + vars = [vars; vec(Rrel_matrix)] if isroot append!(eqs, connect_orientation(ori(frame_b), absolute_rotation(frame_a, Rrel); iscut)) else + vars = [vars; vec(Rrel_inv_matrix)] append!(eqs, [Rrel_inv ~ inverse_rotation(Rrel) connect_orientation(ori(frame_a), absolute_rotation(frame_b, Rrel_inv); iscut)]) @@ -566,19 +573,10 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi end else - # Free motion joint does not have state - if w_rel_a_fixed || z_rel_a_fixed - append!(eqs, - w_rel_b .~ angular_velocity2(ori(frame_b)) - resolve2(ori(frame_b), angular_velocity1(ori(frame_a)))) - end - end - if state && !isroot - compose(System(eqs, t, vars, pars; name), frame_a, frame_b, Rrel_f, Rrel_inv_f) - elseif state - compose(System(eqs, t, vars, pars; name), frame_a, frame_b, Rrel_f, ) - else - compose(System(eqs, t, vars, pars; name), frame_a, frame_b) + append!(eqs, + w_rel_b .~ angular_velocity2(ori(frame_b)) - resolve2(ori(frame_b), angular_velocity1(ori(frame_a)))) end + compose(System(eqs, t, vars, pars; name), frame_a, frame_b) end """ @@ -635,6 +633,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw, state_priority = -1) Rb = ori(frame_b) + vars = [vars; vec(Rrel.R)] eqs = [ Rrel ~ relative_rotation(ori(frame_a), ori(frame_b)) @@ -710,18 +709,18 @@ s_y=prismatic_y.s=0` and `phi=revolute.phi=0`. systems = @named begin frame_a = Frame() frame_b = Frame() - prismatic_x = Prismatic(; state_priority=2.1, n=cross(cross(n, n_x), n), color=boxcolor, radius) - prismatic_y = Prismatic(; state_priority=2.1, n=cross(n, n_x), color=boxcolor, radius) - revolute = Revolute(; state_priority=2.1, n, isroot=false, color=cylindercolor, radius) + prismatic_x = Prismatic(; state_priority=2, n=cross(cross(n, n_x), n), color=boxcolor, radius) + prismatic_y = Prismatic(; state_priority=2, n=cross(n, n_x), color=boxcolor, radius) + revolute = Revolute(; state_priority=2, n, isroot=false, color=cylindercolor, radius) end vars = @variables begin - (s_x(t) = 0), [state_priority = 3.0, description = "Relative distance along first prismatic joint starting at frame_a"] - (s_y(t) = 0), [state_priority = 3.0, description = "Relative distance along second prismatic joint starting at first prismatic joint"] - (phi(t) = 0), [state_priority = 3.0, description = "Relative rotation angle from frame_a to frame_b"] - (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"] + (s_x(t) = 0), [state_priority = 3, description = "Relative distance along first prismatic joint starting at frame_a"] + (s_y(t) = 0), [state_priority = 3, description = "Relative distance along second prismatic joint starting at first prismatic joint"] + (phi(t) = 0), [state_priority = 3, description = "Relative rotation angle from frame_a to frame_b"] + (v_x(t) = 0), [state_priority = 3, description = "Relative velocity along first prismatic joint"] + (v_y(t) = 0), [state_priority = 3, description = "Relative velocity along second prismatic joint"] + (w(t) = 0), [state_priority = 3, description = "Relative angular velocity 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"] @@ -883,4 +882,4 @@ end ] return System(equations, t, vars, pars; name, systems) -end \ No newline at end of file +end diff --git a/src/orientation.jl b/src/orientation.jl index de848a89..f9119dc8 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -22,7 +22,6 @@ const R3{T} = RotMatrix{3, T} # w::Any # end -RotationMatrix(R::AbstractMatrix, w) = RotationMatrix(R3(R), w) RotationMatrix() = RotationMatrix(R3(1.0I(3)), zeros(3)) @@ -436,9 +435,10 @@ function get_frame(sol, frame, t) end function nonunit_quaternion_equations(R, w; Q = [1, 0, 0, 0]) + Q0 = Q vars = @variables begin Q(t)[1:4], [state_priority=-1, description="Unit quaternion with [w,i,j,k]"] # normalized - Q̂(t)[1:4]=Q, [state_priority=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized + Q̂(t)[1:4]=Q0, [state_priority=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized Q̂d(t)[1:4], [state_priority=1000] # NOTE: n(t) @@ -460,11 +460,11 @@ function nonunit_quaternion_equations(R, w; Q = [1, 0, 0, 0]) # QR = from_Q(Q, angular_velocity2(Q, D.(Q))) QR = from_Q(Q̂ ./ sqrt(n), w) eqs = [ - n ~ Q̂'Q̂ + n ~ sum(abs2, collect(Q̂)) c ~ k * (1 - n) D.(Q̂) .~ Q̂d - Q̂d .~ (Ω * Q̂) ./ 2 + c * Q̂ # We use Ω' which is the same as using -w to handle the fact that w coming in here is typically described frame_a rather than in frame_b, the paper is formulated with w being expressed in the rotating body frame (frame_b) - Q .~ Q̂ ./ sqrt(n) + collect(Q̂d) .~ (Ω * collect(Q̂)) ./ 2 + c * collect(Q̂) # We use Ω' which is the same as using -w to handle the fact that w coming in here is typically described frame_a rather than in frame_b, the paper is formulated with w being expressed in the rotating body frame (frame_b) + collect(Q) .~ collect(Q̂) ./ sqrt(n) R ~ from_Q(Q, w) # R.w .~ angularVelocity2(Q̂, Q̂d) ] diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 0ebc89c3..098d32bb 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -420,12 +420,12 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) # end @variables begin - (q(t)[1:6]), [guess = 0, state_priority = 2, description = "Joint angles"] - (qd(t)[1:6]), [guess = 0, state_priority = 2, description = "Joint speeds"] + (q(t)[1:6]), [guess = zeros(6), state_priority = 2, description = "Joint angles"] + (qd(t)[1:6]), [guess = zeros(6), state_priority = 2, description = "Joint speeds"] (qdd(t)[1:6]), - [guess = 0, state_priority = 2, description = "Joint accelerations"] + [guess = zeros(6), state_priority = 2, description = "Joint accelerations"] (tau(t)[1:6]), - [guess = 0, description = "Joint driving torques"] + [guess = zeros(6), description = "Joint driving torques"] end path = @__DIR__() diff --git a/test/basic_tests.jl b/test/basic_tests.jl index a4f5e4d5..4930d84b 100644 --- a/test/basic_tests.jl +++ b/test/basic_tests.jl @@ -1,3 +1,4 @@ +using ADTypes using ModelingToolkit using Multibody using Test @@ -58,7 +59,7 @@ D = Differential(t) @test sol(2pi, idxs = body.r_0[1])≈0 atol=1e-3 @test sol(2pi, idxs = body.r_0[2])≈0 atol=1e-3 @test sol(2pi, idxs = body.r_0[3])≈0 atol=1e-3 - @test sol(pi, idxs = body.r_0[2]) < -2 + @test sol(float(pi), idxs = body.r_0[2]) < -2 doplot() && plot(sol, idxs = [collect(body.r_0); collect(body.v_0)], layout = 6) @@ -121,9 +122,10 @@ end # ============================================================================== ## Point gravity ====================== # ============================================================================== +@testset "Point gravity" begin @component function PointGrav(; name) systems = @named begin - world = World() + world = World(mu = 1, point_gravity = true) body1 = Body( m=1, I_11=0.1, @@ -155,20 +157,15 @@ end @named model = PointGrav() model = complete(model) ssys = multibody(model) -defs = [ - model.world.mu => 1 - model.world.point_gravity => true - # collect(model.body1.w_a) .=> 0 - # collect(model.body2.w_a) .=> 0 - -] -prob = ODEProblem(ssys, defs, (0, 5)) +prob = ODEProblem(ssys, [], (0, 5)) sol = solve(prob, Rodas4()) @test sol(5, idxs=model.body2.r_0) ≈ [0.7867717, 0.478463, 0] atol=1e-1 # plot(sol) +end + # ============================================================================== ## Simple pendulum from Modelica "First Example" tutorial ====================== # ============================================================================== @@ -224,7 +221,7 @@ connections = [connect(world.frame_b, rev.frame_a) ssys = multibody(model)#, alias_eliminate = false) D = Differential(t) -prob = ODEProblem(ssys, [damper.phi_rel => 1, D(rev.phi) => 0, D(D(rev.phi)) => 0], +prob = ODEProblem(ssys, [rev.phi => 0, rev.w => 0], (0, 100)) sol2 = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol2) @@ -258,7 +255,7 @@ ssys = multibody(model) D = Differential(t) -prob = ODEProblem(ssys, [damper.phi_rel => 1], (0, 40)) +prob = ODEProblem(ssys, [rev.phi => 0, rev.w => 0], (0, 40)) sol3 = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol3) @test minimum(sol3[rev.phi]) > -π @@ -343,7 +340,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = System(connections, t, systems = [world, joint, body, damper, spring]) ssys = multibody(model)#, allow_parameter = false) -prob = ODEProblem(ssys, [damper.s_rel => 1, D(joint.s) => 0, D(D(joint.s)) => 0], +prob = ODEProblem(ssys, [joint.s => 1, joint.v => 0], (0, 30)) sol = solve(prob, Rodas4()) @@ -366,7 +363,7 @@ systems = @named begin bar1 = FixedTranslation(r = [0.3, 0, 0]) bar2 = FixedTranslation(r = [0.6, 0, 0]) bar3 = FixedTranslation(r = [0.9, 0, 0]) - p2 = Prismatic(n = [0, -1, 0], s = 0.1, axisflange = true) + p2 = Prismatic(n = [0, -1, 0], s = 0.1, v = 0, axisflange = true) spring2 = Multibody.Spring(c = 30, s_unstretched = 0.1) spring1 = Multibody.Spring(c = 30, s_unstretched = 0.1) damper1 = Multibody.Damper(d = 2) @@ -532,9 +529,10 @@ end ## Planar joint ================================================================ # ============================================================================== using LinearAlgebra +@testset "Planar joint" begin @component function PlanarTest(; name) systems = @named begin - world = World() + world = World(g = 9.80665) planar = Planar(n=[0,0,1], n_x=[1,0,0]) force = Force() body = Body(m=1) @@ -560,11 +558,7 @@ end @named sys = PlanarTest() sys = complete(sys) ssys = multibody(sys) -prob = ODEProblem(ssys, [ - sys.world.g => 9.80665; # Modelica default - # collect(sys.body.w_a) .=> 0; - # collect(sys.body.v_0) .=> 0; -], (0, 2)) +prob = ODEProblem(ssys, [], (0, 2)) sol = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol) @@ -574,6 +568,8 @@ sol = solve(prob, Rodas4()) # plot(sol) +end + # ============================================================================== ## Sperical-joint pendulum =================================================== # ============================================================================== @@ -589,7 +585,7 @@ D = Differential(t) world = Multibody.world @named begin - joint = Spherical(state = true, isroot = true, phi = 1) + joint = Spherical(state = true, isroot = true, phi = fill(1.0, 3)) bar = FixedTranslation(r = [0, -1, 0]) body = Body(; m = 1, isroot = false) end @@ -726,7 +722,7 @@ end @testset "FreeMotion" begin # Model a free-falling body # Test 1, enforce state = false -world = Multibody.world +world = World(; name = :world, g = 9.81) @named freeMotion = FreeMotion(state = false, isroot = false) @named body = Body(m = 1, isroot = true) @@ -741,7 +737,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) ssys = multibody(model) @test length(unknowns(ssys)) == 12 -prob = ODEProblem(ssys, [world.g=>9.81; collect(body.w_a .=> [0, 0, 0]); collect(body.v_0 .=> [0, 0, 0]); ], (0, 10)) +prob = ODEProblem(ssys, [ collect(body.w_a .=> [0, 0, 0]); collect(body.v_0 .=> [0, 0, 0]); ], (0, 10)) sol = solve(prob, Rodas4()) doplot() && plot(sol, idxs = body.r_0[2], title = "Free falling body") @@ -765,7 +761,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) ssys = multibody(model) @test length(unknowns(ssys)) == 12 -prob = ODEProblem(ssys, [world.g=>9.81; collect(body.w_a .=> [0, 1, 0]); collect(body.v_0 .=> [0, 0, 0]); ], (0, 10)) +prob = ODEProblem(ssys, [ collect(body.v_0 .=> [0, 0, 0]); ], (0, 10)) sol = solve(prob, Rodas4()) doplot() && plot(sol, idxs = body.r_0[2], title = "Free falling body") @@ -774,10 +770,10 @@ y = sol(0:0.1:10, idxs = body.r_0[2]) # without joint -world = Multibody.world +world = World(; name = :world, g = 9.81) @named body = Body(m = 1, isroot = true) -@named model = System([], t, +@named model = System(Equation[], t, systems = [world; body]) # ssys = structural_simplify(model, allow_parameters = false) @@ -785,7 +781,6 @@ ssys = multibody(model) @test length(unknowns(ssys)) == 12 prob = ODEProblem(ssys, [ - world.g=>9.81; collect(body.w_a .=> [0, 1, 0]); collect(body.v_0 .=> [0, 0, 0]); ], (0, 10)) @@ -801,7 +796,7 @@ end ## Dzhanibekov effect ========================================================== # ============================================================================== @testset "Dzhanibekov effect" begin -world = Multibody.world +world = World(; name = :world, g = 0.0) @named freeMotion = FreeMotion(state = false, isroot = false) @named body = BodyShape(m = 1, r=0.1*[1,2,3], isroot = true, I_11 = 1, I_22 = 10, I_33 = 100, quat=false) @@ -816,13 +811,9 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) ssys = multibody(model) prob = ODEProblem(ssys, - Symbolics.scalarize.([ - body.body.v_0 .=> [0,1,0] # Movement in y direction - body.body.w_a .=> [0.01, 3.0, 0.02] # Rotation around y axis - body.body.phi .=> 0.0001 .* randn.() - # body.Q .=> 0.001 .* randn.() .+ [0,0,0,1] - world.g => 0.0 # In space - ]), + [collect(body.body.v_0) .=> [0, 1, 0]; + collect(body.body.w_a) .=> [0.01, 3.0, 0.02]; + collect(body.body.phi) .=> 0.0001 .* randn(3)], (0, 3)) sol = solve(prob, Rodas4()) @@ -877,9 +868,8 @@ BSine = Blocks.Sine end @named model = ActuatedJoint() -cm = complete(model) ssys = multibody(model) -prob = ODEProblem(ssys, [D(D(cm.joint.phi)) => 0], (0, 10)) +prob = ODEProblem(ssys, [ssys.joint.phi => 0, ssys.joint.w => 0], (0, 10)) sol = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol) doplot() && plot(sol) |> display @@ -916,7 +906,7 @@ prob = ODEProblem(ssys, [ collect(body.r_0) .=> [1,1,1]; collect(body.w_a) .=> [1,1,1] ], (0, 5)) -@time "Stiff rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ 0.1); +@time "Stiff rope pendulum" sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff()); u0 = prob.u0 .+ 0.1); @test SciMLBase.successful_retcode(sol) @@ -947,7 +937,7 @@ prob = ODEProblem(ssys, [ collect(body.w_a) .=> [1,1,1]; collect(body.v_0) .=> [10,10,10] ], (0, 10)) -@time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ [-0.5, -0.5, -0.5, +@time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff()); u0 = prob.u0 .+ [-0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, 0.5, @@ -1039,7 +1029,7 @@ end collect(chain.link_3.body.w_a) .=> [0,0,0]; collect(chain.link_3.frame_b.r_0) .=> [x_dist,0,0]; ], (0, 4)) - sol = solve(prob, Rodas4(autodiff=false)) + sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff())) @test SciMLBase.successful_retcode(sol) # Multibody.render(mounted_chain, sol, x=3, filename = "mounted_chain.gif") # May take long time for n>=10 @@ -1167,7 +1157,7 @@ world = Multibody.world @named joint = Multibody.Spherical(isroot=false, state=false, quat=false) @named rod = FixedTranslation(; r = [1, 0, 0]) -@named body = Body(; m = 1, isroot=true, quat=true) +@named body = Body(; m = 1, isroot=true, quat=true, r_0=[1, 0, 0]) connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, rod.frame_a) @@ -1487,7 +1477,7 @@ display(sort(unknowns(ssys), by=string)) prob = ODEProblem(ssys, defs, (0, 2)) -sol = solve(prob, FBDF(autodiff=true))#; initializealg=ShampineCollocationInit()) +sol = solve(prob, FBDF(autodiff=AutoForwardDiff()))#; initializealg=ShampineCollocationInit()) @test SciMLBase.successful_retcode(sol) # Multibody.render(model, sol, show_axis=false, x=-1.5, y=0, z=0, timescale=3, display=true) # Video # first(Multibody.render(model, sol, 0, show_axis=true, x=-1.5, y=0, z=0)) @@ -1611,7 +1601,7 @@ display(sort(unknowns(ssys), by=string)) prob = ODEProblem(ssys, defs, (0, 2)) -sol = solve(prob, FBDF(autodiff=true))#; initializealg=ShampineCollocationInit()) +sol = solve(prob, FBDF(autodiff=AutoForwardDiff()))#; initializealg=ShampineCollocationInit()) @test SciMLBase.successful_retcode(sol) # Multibody.render(model, sol, show_axis=false, x=-1.5, y=0, z=0, timescale=3, display=true) # Video # first(Multibody.render(model, sol, 0, show_axis=true, x=-1.5, y=0, z=0)) diff --git a/test/runtests.jl b/test/runtests.jl index 39418c31..309a2dfe 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -3,6 +3,7 @@ using Multibody using Test # using JuliaSimCompiler using OrdinaryDiffEq +using OrdinaryDiffEqRosenbrock using LinearAlgebra isdefined(Main, :t) || (t = Multibody.t) isdefined(Main, :D) || (D = Differential(t)) @@ -59,4 +60,13 @@ end @info "Testing basic_tests" include("basic_tests.jl") end + +@testset "CraigBampton" begin + include("test_craig_bampton.jl") +end + +include("test_bodyshape_initialization.jl") +include("test_joint_array_parameters.jl") +include("test_joint_orientation.jl") +include("test_linear_scc.jl") end diff --git a/test/test_JointUSR_RRR.jl b/test/test_JointUSR_RRR.jl index b8dbb6b7..89a85dd1 100644 --- a/test/test_JointUSR_RRR.jl +++ b/test/test_JointUSR_RRR.jl @@ -1,3 +1,4 @@ +using ADTypes using Test using Multibody using ModelingToolkit @@ -47,7 +48,7 @@ ssys = multibody(model) ## prob = ODEProblem(ssys, [model.p1.v => 0.0], (0.0, 1.4)) -sol = solve(prob, FBDF(autodiff=true)) +sol = solve(prob, FBDF(autodiff=AutoForwardDiff())) @test sol(1.0, idxs=model.p1.s) ≈ -2.8 rtol=0.01 # test vs. OpenModelica @@ -87,6 +88,6 @@ ssys = multibody(model) ## prob = ODEProblem(ssys, [model.p1.v => 0.0], (0.0, 1.4)) -sol = solve(prob, FBDF(autodiff=true)) +sol = solve(prob, FBDF(autodiff=AutoForwardDiff())) @test sol(1.0, idxs=model.p1.s) ≈ -2.8 rtol=0.01 # test vs. OpenModelica diff --git a/test/test_OneDOFWheelJoint.jl b/test/test_OneDOFWheelJoint.jl index 14903f79..19b6a672 100644 --- a/test/test_OneDOFWheelJoint.jl +++ b/test/test_OneDOFWheelJoint.jl @@ -1,3 +1,4 @@ +using ADTypes using ModelingToolkit, OrdinaryDiffEq, Test using ModelingToolkit: t_nounits as t, D_nounits as D import ModelingToolkitStandardLibrary.Blocks @@ -91,7 +92,7 @@ g = -9.80665 missing_guess_value = MissingGuessValue.Constant(0.0), ) - sol = solve(prob, Rodas5P(autodiff=true)) + sol = solve(prob, Rodas5P(autodiff=AutoForwardDiff())) @test sol(0.1, idxs=ssys.body.frame_a.phi - ssys.wheelJoint.frame_a.phi) == 0 # These two should be the same @test sol(0.1, idxs=ssys.body.w + ssys.wheelJoint.w_roll) == 0 # These two should be the same but opposite sign diff --git a/test/test_PlanarMechanics.jl b/test/test_PlanarMechanics.jl index 34e76e45..815428a9 100644 --- a/test/test_PlanarMechanics.jl +++ b/test/test_PlanarMechanics.jl @@ -1,6 +1,6 @@ # using Revise # using Plots -using ModelingToolkit, OrdinaryDiffEq, Test +using ModelingToolkit, OrdinaryDiffEq, Test, ADTypes using ModelingToolkit: t_nounits as t, D_nounits as D import ModelingToolkitStandardLibrary.Blocks import ModelingToolkitStandardLibrary.Mechanical.Rotational @@ -487,7 +487,7 @@ import ModelingToolkitStandardLibrary.Mechanical.Rotational display(unknowns(ssys)) prob = ODEProblem(ssys, [ ], (0.0, 20.0)) - sol = solve(prob, Rodas5Pr(autodiff=true)) # Since the friction model is not differentiable everywhere + sol = solve(prob, Rodas5Pr(autodiff=AutoForwardDiff())) # Since the friction model is not differentiable everywhere @test sol(15, idxs=[ssys.slipBasedWheelJoint.f_lat, ssys.slipBasedWheelJoint.f_long]) ≈ [80, -5] rtol=0.01 @test sol(20, idxs=[ssys.slipBasedWheelJoint.f_lat, ssys.slipBasedWheelJoint.f_long]) ≈ [80, -4.95] rtol=0.01 @@ -611,7 +611,7 @@ end @named model = TwoTrackWithDifferentialGear() ssys = multibody(model) prob = ODEProblem(ssys, [], (0.0, 20.0)) -sol = solve(prob, Rodas5P(autodiff=false)) +sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff())) @test SciMLBase.successful_retcode(sol) # plot(sol) diff --git a/test/test_bodyshape_initialization.jl b/test/test_bodyshape_initialization.jl new file mode 100644 index 00000000..8f9a4c53 --- /dev/null +++ b/test/test_bodyshape_initialization.jl @@ -0,0 +1,15 @@ +using ModelingToolkit, Multibody, Test + +@testset "BodyShape initialization" begin + @named world = World() + @named joint = Prismatic(n = [1, 0, 0]) + @named cart = BodyShape(m = 50.0, r = [0.4, 0, 0]) + eqs = [ + connect(world.frame_b, joint.frame_a), + connect(joint.frame_b, cart.frame_a) + ] + @named model = System(eqs, Multibody.t; systems = [world, joint, cart]) + ssys = multibody(model) + prob = ODEProblem(ssys, [joint.s => 0.0, joint.v => 0.0], (0.0, 1.0)) + @test length(prob.u0) == 2 +end diff --git a/test/test_craig_bampton.jl b/test/test_craig_bampton.jl new file mode 100644 index 00000000..86236f30 --- /dev/null +++ b/test/test_craig_bampton.jl @@ -0,0 +1,522 @@ +using Multibody +using ModelingToolkit +using OrdinaryDiffEq +using LinearAlgebra +using Test + +t = Multibody.t +D = Differential(t) +world = Multibody.world + +@testset "Single boundary modal body" begin + M_BI = zeros(6, 1) + M_BI[1, 1] = 0.1 + @named cb = CraigBampton( + boundary_positions = [[0.0, 0.0, 0.0]], + M_BB = Diagonal([1.0, 1.0, 1.0, 0.1, 0.1, 0.1]), + M_BI = M_BI, + M_II = I(1), + K_BB = zeros(6, 6), + K_II = Diagonal([100.0]), + ζ = [0.02], + ω = [10.0], + ) + @named model = System([connect(world.frame_b, cb.frame_a)], t; systems = [world, cb]) + ssys = multibody(model) + prob = ODEProblem(ssys, [cb.η[1] => 0.01], (0.0, 1.0)) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + @test abs(sol[cb.η[1]][end]) < abs(sol[cb.η[1]][1]) +end + + +@testset "Component construction" begin + # Test basic construction with 2 boundaries and 2 modes + n_boundaries = 2 + n_modes = 2 + n_bdof = 6 * n_boundaries + + # Simple test matrices (identity-ish for testing) + M_BB = 0.1 * I(n_bdof) + M_BI = 0.01 * ones(n_bdof, n_modes) + M_II = I(n_modes) # Mass-normalized modes + K_BB = 100.0 * I(n_bdof) + ω_modes = [10.0, 25.0] # rad/s + K_II = Diagonal(ω_modes.^2) + ζ = [0.02, 0.02] # 2% damping + + @named cb = CraigBampton( + boundary_positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ, + ω = ω_modes, + ) + + @test cb isa System + @test hasproperty(cb, :frame_a) + @test hasproperty(cb, :frame_b) +end + +@testset "Simulation with fixed boundary" begin + # Test a simple case: CB component fixed at frame_a, free at frame_b + n_boundaries = 2 + n_modes = 2 + n_bdof = 6 * n_boundaries + + # Create simple CB matrices for a cantilever-like beam + # Boundary 1 (frame_a) at origin, Boundary 2 (frame_b) at x=1 + M_BB = Diagonal(vcat( + [1.0, 1.0, 1.0, 0.1, 0.1, 0.1], # Boundary 1 (fixed, so won't matter much) + [0.5, 0.5, 0.5, 0.05, 0.05, 0.05] # Boundary 2 mass + )) + M_BI = zeros(n_bdof, n_modes) + # Coupling: boundary 2 y-displacement couples to mode 1 (bending mode) + M_BI[8, 1] = 0.1 # y-displacement at boundary 2, mode 1 + M_BI[9, 2] = 0.05 # z-displacement at boundary 2, mode 2 + + M_II = I(n_modes) + + # Stiffness + K_BB = Diagonal(vcat( + [1e6, 1e6, 1e6, 1e4, 1e4, 1e4], # Boundary 1 (high stiffness for fixed) + [100.0, 100.0, 100.0, 10.0, 10.0, 10.0] # Boundary 2 flexible + )) + + ω_modes = [10.0, 25.0] # Fundamental frequencies + K_II = Diagonal(ω_modes.^2) + ζ = [0.05, 0.05] # 5% damping + + @named cb = CraigBampton( + boundary_positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ, + ω = ω_modes, + ) + + # Fix frame_a to world + eqs = [ + connect(world.frame_b, cb.frame_a) + ] + + @named model = System(eqs, t; systems=[world, cb]) + + # Try to build the system + ssys = multibody(model) + + @test ssys isa System + + # The system should have states for η, η̇, q_flex, q̇_flex + # n_modes * 2 + (n_boundaries - 1) * 6 * 2 = 2*2 + 1*6*2 = 16 DOFs before simplification + + # Create and solve ODE problem with small initial deformation + prob = ODEProblem(ssys, [ + cb.η[1] => 0.01, # Small initial modal amplitude + cb.η[2] => 0.0, + ], (0.0, 1.0)) + + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + + # The modal amplitude should decay due to damping + η1_initial = sol[cb.η[1]][1] + η1_final = sol[cb.η[1]][end] + @test abs(η1_final) < abs(η1_initial) # Should decay +end + +@testset "Matrix size validation" begin + n_boundaries = 2 + n_modes = 3 + n_bdof = 6 * n_boundaries + + # Correct sizes + M_BB = zeros(n_bdof, n_bdof) + M_BI = zeros(n_bdof, n_modes) + M_II = I(n_modes) + K_BB = zeros(n_bdof, n_bdof) + K_II = I(n_modes) + ζ = zeros(n_modes) + ω = ones(n_modes) + + # Should succeed + @named cb_ok = CraigBampton( + boundary_positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ, + ω = ω, + ) + @test cb_ok isa System + + # Wrong M_BB size should fail + @test_throws AssertionError CraigBampton( + name = :cb_bad, + boundary_positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = zeros(10, 10), # Wrong size + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ, + ω = ω, + ) + + # Wrong ζ length should fail + @test_throws AssertionError CraigBampton( + name = :cb_bad2, + boundary_positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = [0.01, 0.02], # Wrong length (2 instead of 3) + ω = ω, + ) +end + +@testset "Three boundary component" begin + # Test with 3 boundaries + n_boundaries = 3 + n_modes = 2 + n_bdof = 6 * n_boundaries + + M_BB = 0.1 * I(n_bdof) + M_BI = zeros(n_bdof, n_modes) + M_II = I(n_modes) + K_BB = 100.0 * I(n_bdof) + K_II = Diagonal([100.0, 400.0]) + ζ = [0.02, 0.02] + ω = [10.0, 20.0] + + @named cb3 = CraigBampton( + boundary_positions = [[0.0, 0.0, 0.0], [0.5, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ, + ω = ω, + ) + + @test cb3 isa System + @test hasproperty(cb3, :frame_a) + @test hasproperty(cb3, :frame_b) + @test hasproperty(cb3, :frame_c) +end + +# ========================================================================= +# Analytical Solution Tests +# ========================================================================= +# These tests verify the modal dynamics against known analytical solutions +# for the damped harmonic oscillator: η̈ + 2ζω*η̇ + ω²*η = 0 + +""" +Helper function to create a minimal CB component for analytical testing. +Uses M_BI = 0 to decouple modal dynamics from boundary dynamics. +""" +function create_decoupled_cb(; n_modes, ω_modes, ζ_modes, name) + n_boundaries = 2 + n_bdof = 6 * n_boundaries + + # Decoupled system: M_BI = 0 means modes evolve independently + M_BB = 0.1 * I(n_bdof) + M_BI = zeros(n_bdof, n_modes) + M_II = Matrix(1.0 * I(n_modes)) # Mass-normalized modes + K_BB = 100.0 * I(n_bdof) + K_II = Diagonal(ω_modes.^2) + + CraigBampton(; + name, + boundary_positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], + M_BB = M_BB, + M_BI = M_BI, + M_II = M_II, + K_BB = K_BB, + K_II = K_II, + ζ = ζ_modes, + ω = ω_modes, + ) +end + +""" +Analytical solution for damped harmonic oscillator with IC: η(0)=η₀, η̇(0)=0 +""" +function analytical_damped_oscillator(t, η₀, ω, ζ) + if ζ < 1 # Underdamped + ωd = ω * sqrt(1 - ζ^2) + return η₀ * exp(-ζ * ω * t) * (cos(ωd * t) + (ζ / sqrt(1 - ζ^2)) * sin(ωd * t)) + elseif ζ == 1 # Critically damped + return η₀ * exp(-ω * t) * (1 + ω * t) + else # Overdamped + r1 = -ω * (ζ + sqrt(ζ^2 - 1)) + r2 = -ω * (ζ - sqrt(ζ^2 - 1)) + A = η₀ * r2 / (r2 - r1) + B = -η₀ * r1 / (r2 - r1) + return A * exp(r1 * t) + B * exp(r2 * t) + end +end + +""" +Analytical solution for damped oscillator with IC: η(0)=0, η̇(0)=v₀ +""" +function analytical_velocity_ic(t, v₀, ω, ζ) + ωd = ω * sqrt(1 - ζ^2) + return (v₀ / ωd) * exp(-ζ * ω * t) * sin(ωd * t) +end + +@testset "Undamped single mode - frequency verification" begin + # Test that undamped oscillator has correct frequency + # η(t) = η₀ * cos(ω*t), period T = 2π/ω + ω_test = 10.0 # rad/s + ζ_test = 1e-8 # Essentially undamped (can't use exactly 0) + η₀ = 0.1 + + cb = create_decoupled_cb(n_modes=1, ω_modes=[ω_test], ζ_modes=[ζ_test], name=:cb_undamped) + + eqs = [connect(world.frame_b, cb.frame_a)] + @named model = System(eqs, t; systems=[world, cb]) + ssys = multibody(model) + + # Simulate for multiple periods + T_period = 2π / ω_test + t_end = 5 * T_period + prob = ODEProblem(ssys, [cb.η[1] => η₀], (0.0, t_end)) + sol = solve(prob, Rodas5P(); saveat=T_period/100) + + @test SciMLBase.successful_retcode(sol) + + # Compare to analytical solution at sample times + for (i, ti) in enumerate(sol.t[1:min(100, end)]) + η_numerical = sol[cb.η[1]][i] + η_analytical = η₀ * cos(ω_test * ti) + @test isapprox(η_numerical, η_analytical, rtol=0.01, atol=0.01) + end + + # Verify period by checking zero crossings + η_vals = sol[cb.η[1]] + # Find time of first positive-to-negative crossing (should be at T/4) + for i in 2:length(η_vals)-1 + if η_vals[i] > 0 && η_vals[i+1] < 0 + # Linear interpolation for zero crossing + t_cross = sol.t[i] + (0 - η_vals[i]) / (η_vals[i+1] - η_vals[i]) * (sol.t[i+1] - sol.t[i]) + # First zero crossing of cos(ωt) is at T/4 + expected_cross = T_period / 4 + @test isapprox(t_cross, expected_cross, rtol=0.02, atol=0.01) + break + end + end +end + +@testset "Damped oscillator - exponential envelope" begin + # Verify that envelope decays as exp(-ζ*ω*t) + ω_test = 10.0 + ζ_test = 0.05 # 5% damping + η₀ = 0.1 + + cb = create_decoupled_cb(n_modes=1, ω_modes=[ω_test], ζ_modes=[ζ_test], name=:cb_damped_env) + + eqs = [connect(world.frame_b, cb.frame_a)] + @named model = System(eqs, t; systems=[world, cb]) + ssys = multibody(model) + + t_end = 3.0 # Long enough to see significant decay + prob = ODEProblem(ssys, [cb.η[1] => η₀], (0.0, t_end)) + sol = solve(prob, Rodas5P(); saveat=0.01) + + @test SciMLBase.successful_retcode(sol) + + # Check envelope at specific times + test_times = [0.5, 1.0, 1.5, 2.0, 2.5] + for t_check in test_times + idx = findfirst(t -> t >= t_check, sol.t) + if idx !== nothing + η_numerical = sol[cb.η[1]][idx] + η_analytical = analytical_damped_oscillator(sol.t[idx], η₀, ω_test, ζ_test) + @test isapprox(η_numerical, η_analytical, rtol=0.05) + end + end + + # Verify overall decay rate by comparing early and late values + η_early = maximum(abs.(sol[cb.η[1]][1:50])) + η_late = maximum(abs.(sol[cb.η[1]][end-50:end])) + expected_decay = exp(-ζ_test * ω_test * (sol.t[end-25] - sol.t[25])) + actual_decay = η_late / η_early + @test actual_decay < 1.0 # Should definitely decay + @test isapprox(actual_decay, expected_decay, rtol=0.3) # Within 30% of expected +end + +@testset "Logarithmic decrement" begin + # δ = ln(η(t)/η(t+Td)) = 2πζ/√(1-ζ²) + ω_test = 8.0 # Lower frequency for clearer peaks + ζ_test = 0.08 # 8% damping (larger for clearer decay) + η₀ = 0.1 + + cb = create_decoupled_cb(n_modes=1, ω_modes=[ω_test], ζ_modes=[ζ_test], name=:cb_logdec) + + eqs = [connect(world.frame_b, cb.frame_a)] + @named model = System(eqs, t; systems=[world, cb]) + ssys = multibody(model) + + ωd = ω_test * sqrt(1 - ζ_test^2) + Td = 2π / ωd # Damped period + t_end = 6 * Td + + prob = ODEProblem(ssys, [cb.η[1] => η₀], (0.0, t_end)) + sol = solve(prob, Rodas5P(); saveat=Td/50) + + @test SciMLBase.successful_retcode(sol) + + # Find peaks (local maxima) + η_vals = sol[cb.η[1]] + peaks = Float64[] + peak_times = Float64[] + for i in 2:length(η_vals)-1 + if η_vals[i] > η_vals[i-1] && η_vals[i] > η_vals[i+1] && η_vals[i] > 0.01 * η₀ + push!(peaks, η_vals[i]) + push!(peak_times, sol.t[i]) + end + end + + # Need at least 2 peaks to compute log decrement + if length(peaks) >= 2 + # Compute logarithmic decrement from first two peaks + δ_measured = log(peaks[1] / peaks[2]) + δ_expected = 2π * ζ_test / sqrt(1 - ζ_test^2) + @test isapprox(δ_measured, δ_expected, rtol=0.15) + end +end + +@testset "Damped natural frequency" begin + # Verify damped frequency ωd = ω*√(1-ζ²) + ω_test = 12.0 + ζ_test = 0.1 # 10% damping + η₀ = 0.1 + + cb = create_decoupled_cb(n_modes=1, ω_modes=[ω_test], ζ_modes=[ζ_test], name=:cb_damped_freq) + + eqs = [connect(world.frame_b, cb.frame_a)] + @named model = System(eqs, t; systems=[world, cb]) + ssys = multibody(model) + + ωd_expected = ω_test * sqrt(1 - ζ_test^2) + Td_expected = 2π / ωd_expected + t_end = 5 * Td_expected + + prob = ODEProblem(ssys, [cb.η[1] => η₀], (0.0, t_end)) + sol = solve(prob, Rodas5P(); saveat=Td_expected/100) + + @test SciMLBase.successful_retcode(sol) + + # Find zero crossings (positive to negative) to measure period + η_vals = sol[cb.η[1]] + zero_crossings = Float64[] + for i in 1:length(η_vals)-1 + if η_vals[i] > 0 && η_vals[i+1] <= 0 + # Linear interpolation + t_cross = sol.t[i] - η_vals[i] * (sol.t[i+1] - sol.t[i]) / (η_vals[i+1] - η_vals[i]) + push!(zero_crossings, t_cross) + end + end + + # Period is time between consecutive positive-to-negative crossings + if length(zero_crossings) >= 2 + Td_measured = zero_crossings[2] - zero_crossings[1] # One full period + @test isapprox(Td_measured, Td_expected, rtol=0.02) + end +end + +@testset "Multi-mode independence" begin + # Two modes with different frequencies should oscillate independently + ω1 = 10.0 + ω2 = 25.0 + ζ1 = 0.02 + ζ2 = 0.03 + η₀_1 = 0.1 + η₀_2 = 0.05 + + cb = create_decoupled_cb(n_modes=2, ω_modes=[ω1, ω2], ζ_modes=[ζ1, ζ2], name=:cb_multimode) + + eqs = [connect(world.frame_b, cb.frame_a)] + @named model = System(eqs, t; systems=[world, cb]) + ssys = multibody(model) + + t_end = 2.0 + prob = ODEProblem(ssys, [ + cb.η[1] => η₀_1, + cb.η[2] => η₀_2, + ], (0.0, t_end)) + sol = solve(prob, Rodas5P(); saveat=0.005) + + @test SciMLBase.successful_retcode(sol) + + # Each mode should match its own analytical solution + for (i, ti) in enumerate(sol.t) + # Mode 1 + η1_numerical = sol[cb.η[1]][i] + η1_analytical = analytical_damped_oscillator(ti, η₀_1, ω1, ζ1) + @test isapprox(η1_numerical, η1_analytical, rtol=0.05, atol=1e-4) + + # Mode 2 + η2_numerical = sol[cb.η[2]][i] + η2_analytical = analytical_damped_oscillator(ti, η₀_2, ω2, ζ2) + @test isapprox(η2_numerical, η2_analytical, rtol=0.05, atol=1e-4) + end +end + +@testset "Initial velocity response" begin + # IC: η(0) = 0, η̇(0) = v₀ + # Solution: η(t) = (v₀/ωd) * exp(-ζ*ω*t) * sin(ωd*t) + ω_test = 10.0 + ζ_test = 0.05 + v₀ = 1.0 # Initial velocity + + cb = create_decoupled_cb(n_modes=1, ω_modes=[ω_test], ζ_modes=[ζ_test], name=:cb_vel_ic) + + eqs = [connect(world.frame_b, cb.frame_a)] + @named model = System(eqs, t; systems=[world, cb]) + ssys = multibody(model) + + ωd = ω_test * sqrt(1 - ζ_test^2) + Td = 2π / ωd + t_end = 3 * Td + + prob = ODEProblem(ssys, [ + cb.η[1] => 0.0, + cb.η̇[1] => v₀, + ], (0.0, t_end)) + sol = solve(prob, Rodas5P(); saveat=0.01) + + @test SciMLBase.successful_retcode(sol) + + # Compare to analytical solution + for (i, ti) in enumerate(sol.t) + η_numerical = sol[cb.η[1]][i] + η_analytical = analytical_velocity_ic(ti, v₀, ω_test, ζ_test) + # Use absolute tolerance near zero, relative elsewhere + if abs(η_analytical) > 0.01 + @test isapprox(η_numerical, η_analytical, rtol=0.05) + else + @test isapprox(η_numerical, η_analytical, atol=0.01) + end + end + + # Verify first peak location (should be at t = arctan(ωd/(ζω)) / ωd for this IC) + # For small damping: peak ≈ Td/4 + η_vals = sol[cb.η[1]] + peak_idx = argmax(η_vals) + t_peak = sol.t[peak_idx] + t_peak_expected = atan(ωd / (ζ_test * ω_test)) / ωd + @test isapprox(t_peak, t_peak_expected, rtol=0.1) +end diff --git a/test/test_fourbar.jl b/test/test_fourbar.jl index b847a881..65ff0491 100644 --- a/test/test_fourbar.jl +++ b/test/test_fourbar.jl @@ -1,3 +1,4 @@ +using ADTypes using Test using Multibody using ModelingToolkit @@ -42,7 +43,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) @@ -68,7 +69,7 @@ connections = [connect(j2.frame_b, b2.frame_a) @named model = System(connections, t, systems = [world; systems]) ssys = multibody(model, allow_symbolic=true) prob = ODEProblem(ssys, [], (0.0, 1.4399), guesses = [ssys.j1.phi => 0]) # The end time is chosen to make the animation below appear to loop forever -sol2 = solve(prob, FBDF(autodiff=true)) # 3.9x faster than above +sol2 = solve(prob, FBDF(autodiff=AutoForwardDiff())) # 3.9x faster than above # plot(sol2, idxs=[j2.s]) # Plot the joint coordinate of the prismatic joint (green in the animation below) diff --git a/test/test_joint_array_parameters.jl b/test/test_joint_array_parameters.jl new file mode 100644 index 00000000..78ba129d --- /dev/null +++ b/test/test_joint_array_parameters.jl @@ -0,0 +1,8 @@ +using ModelingToolkit, Multibody, Test + +@testset "Joint array parameters" begin + gear = GearConstraint(; name = :gear, ratio = 10) + @test gear isa System + universal = Universal(; name = :universal) + @test universal isa System +end diff --git a/test/test_joint_orientation.jl b/test/test_joint_orientation.jl new file mode 100644 index 00000000..a844c338 --- /dev/null +++ b/test/test_joint_orientation.jl @@ -0,0 +1,72 @@ +using ModelingToolkit, Multibody, OrdinaryDiffEq, Test +@testset "Joint orientation flags" begin + for (wfixed, zfixed) in ((false, false), (true, false), (false, true), (true, true)) + @named world = World(g = 0) + @named joint = FreeMotion(state = false, isroot = false, + w_rel_a_fixed = wfixed, z_rel_a_fixed = zfixed) + @named body = Body(state = true, isroot = true) + @named sys = System( + [connect(world.frame_b, joint.frame_a), + connect(joint.frame_b, body.frame_a)], + t; systems = [world, joint, body]) + compiled = multibody(sys) + sol = solve(ODEProblem(compiled, [], (0.0, 0.1)), Tsit5()) + @test SciMLBase.successful_retcode(sol) + @test sol(0.1; idxs = joint.w_rel_b) == zeros(3) + end + for damping in (0.0, 0.2) + @named world = World(g = 0) + @named joint = Spherical(state = false, d = damping) + @named body = Body(state = true, isroot = true) + @named sys = System( + [connect(world.frame_b, joint.frame_a), + connect(joint.frame_b, body.frame_a)], + t; systems = [world, joint, body]) + compiled = multibody(sys) + sol = solve(ODEProblem(compiled, [], (0.0, 0.1)), Tsit5()) + @test SciMLBase.successful_retcode(sol) + @test sol(0.1; idxs = joint.w_rel) == zeros(3) + end +end + +@testset "Rotated spherical damping" begin + for (state, damping) in Iterators.product((false, true), (0.0, 0.002)) + inertia = 0.001 + quaternion = [cos(pi / 4), sin(pi / 4), 0, 0] + @named world = World(g = 0) + @named joint = Spherical(; state, quat = state, d = damping) + @named body = Body(isroot = !state, quat = !state, + I_11 = inertia, I_22 = inertia, I_33 = inertia, + Q = state ? nothing : quaternion, + w_a = state ? nothing : [0, 0, 1]) + @named sys = System( + [connect(world.frame_b, joint.frame_a), + connect(joint.frame_b, body.frame_a)], + t; systems = [world, joint, body]) + compiled = multibody(sys) + initial = state ? + [collect(joint.Q̂) .=> quaternion; collect(joint.w_rel) .=> [0, 0, 1]] : + [] + sol = solve(ODEProblem(compiled, initial, (0.0, 0.1)), + Tsit5(); reltol = 1e-8, abstol = 1e-10) + @test SciMLBase.successful_retcode(sol) + omega = sol(0.1; idxs = body.w_a) + @test sol(0.1; idxs = joint.w_rel)≈omega atol=1e-7 + @test omega≈[0, 0, exp(-damping / inertia * 0.1)] atol=1e-7 + R = reshape(sol(0.1; idxs = vec(ori(body.frame_a).R)), 3, 3) + @test sol(0.1; idxs = joint.frame_b.tau)≈damping * omega atol=1e-7 + @test sol(0.1; idxs = joint.frame_a.tau)≈-R' * (damping * omega) atol=1e-7 + end +end + +@testset "Spherical-spherical scalar constraint" begin + @named world = World(g = 0) + @named rod = SphericalSpherical(r_0 = [1, 0, 0], m = 1, kinematic_constraint = false) + @named body = Body(isroot = true, r_0 = [1, 0, 0]) + @named sys = System( + [connect(world.frame_b, rod.frame_a), + connect(rod.frame_b, body.frame_a)], + t; systems = [world, rod, body]) + compiled = multibody(sys) + @test compiled isa System +end diff --git a/test/test_linear_scc.jl b/test/test_linear_scc.jl new file mode 100644 index 00000000..871bcfc7 --- /dev/null +++ b/test/test_linear_scc.jl @@ -0,0 +1,26 @@ +using ModelingToolkit, Multibody, OrdinaryDiffEq, OrdinaryDiffEqRosenbrock, + Test + +@testset "Numerical linear algebraic loop at axis-aligned orientation" begin + @named world = World() + @named body = Body(m = 1, isroot = true, r_cm = zeros(3), quat = true) + @named spring = Multibody.Spring(c = 1) + @named model = System( + [connect(world.frame_b, spring.frame_a), + connect(spring.frame_b, body.frame_a)], + t; systems = [world, spring, body]) + compiled = multibody(model; + reassemble_alg = ModelingToolkit.StructuralTransformations.DefaultReassembleAlgorithm( + inline_linear_sccs = true, analytical_linear_scc_limit = 1)) + @test length(unknowns(compiled)) == 13 + initial = [collect(body.r_0) .=> [0, -1e-5, 0]; + collect(body.v_0) .=> zeros(3); collect(body.w_a) .=> [0, 1, 0]; + collect(body.Q̂) .=> [1, 0, 0, 0]] + prob = ODEProblem(compiled, initial, (0.0, 10.0)) + du = similar(prob.u0) + prob.f(du, prob.u0, prob.p, 0.0) + @test all(isfinite, du) + sol = solve(prob, Rodas5P()) + @test SciMLBase.successful_retcode(sol) + @test sol(float(pi); idxs = body.r_0[2]) < -2 +end diff --git a/test/test_quaternions.jl b/test/test_quaternions.jl index 7ce0e2bf..a2407365 100644 --- a/test/test_quaternions.jl +++ b/test/test_quaternions.jl @@ -6,7 +6,7 @@ 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) + reshape(sol(t, idxs=vec(collect(ori(frame).R))), 3, 3) end function get_r(sol, frame, t) sol(t, idxs=collect(frame.r_0)) @@ -38,7 +38,7 @@ D = Differential(t) # @test all(isfinite, du) # prob = ODEProblem(ssys, ModelingToolkit.missing_variable_defaults(ssys), (0, 10)) -prob = ODEProblem(ssys, [collect(body.v_0 .=> [0, 0, 0]); collect(body.w_a .=> [0, 1, 0]); collect(body.Q̂ .=> [1, 0, 0, 0])], (0, 10)) +prob = ODEProblem(ssys, [collect(body.r_0) .=> [0, -1e-5, 0]; collect(body.v_0 .=> [0, 0, 0]); collect(body.w_a .=> [0, 1, 0]); collect(body.Q̂ .=> [1, 0, 0, 0])], (0, 10)) sol = solve(prob, Rodas5P(), u0 = prob.u0 .+ 1e-12 .* randn.()) doplot() && @@ -47,7 +47,7 @@ doplot() && @test sol(2pi, idxs = body.r_0[1])≈0 atol=1e-3 @test sol(2pi, idxs = body.r_0[2])≈0 atol=1e-3 @test sol(2pi, idxs = body.r_0[3])≈0 atol=1e-3 -@test sol(pi, idxs = body.r_0[2]) < -2 +@test sol(float(pi), idxs = body.r_0[2]) < -2 # TODO: add more tests ts = 0:0.1:2pi @@ -56,7 +56,7 @@ ti = 5 for (i, ti) in enumerate(ts) R = get_R(sol, body.frame_a, ti) Q = sol(ti, idxs=body.Q) - @test Multibody.from_Q(Q, 0).R ≈ R atol=1e-6 # Test that from_Q yields a rotation matrix consistent with R + @test Symbolics.value.(Multibody.from_Q(Q, zeros(3)).R) ≈ R atol=1e-6 # Test that from_Q yields a rotation matrix consistent with R R2 = get_R(sol, spring.frame_b, ti) @test norm(R'R2 - I) < 1e-10 end @@ -114,7 +114,7 @@ ti = 5 for (i, ti) in enumerate(ts) R = get_R(sol, body.frame_a, ti) @test norm(R'R - I) < 1e-10 - @test Multibody.from_Q(Q[:, i], 0).R ≈ R atol=1e-3 + @test Symbolics.value.(Multibody.from_Q(Q[:, i], zeros(3)).R) ≈ R atol=1e-3 end # Test that rotational velocity of 1 results in one full rotation in 2π seconds. Test rotation around all three major axes @@ -202,7 +202,7 @@ end @named joint = Multibody.Spherical(isroot=false, state=false, quat=false) @named rod = FixedTranslation(; r = [1, 0, 0]) - @named body = Body(; m = 1, isroot=true, quat=true, air_resistance=0.0) + @named body = Body(; m = 1, isroot=true, quat=true, r_0=[1, 0, 0], air_resistance=0.0) # @named joint = Multibody.Spherical(isroot=true, state=true, quat=true) # @named body = Body(; m = 1, r_cm = [1.0, 0, 0], isroot=false) @@ -293,7 +293,7 @@ using Multibody.Rotations: params ssys = multibody(model) # ssys = structural_simplify(model, allow_parameters = false) prob = ODEProblem(ssys, - [world.g => 9.80665; + [ collect(body.body.w_a .=> 0); collect(body.body.v_0 .=> 0); # collect(body.body.phi .=> deg2rad(10)); diff --git a/test/test_robot.jl b/test/test_robot.jl index e0e1d41b..82bf1dde 100644 --- a/test/test_robot.jl +++ b/test/test_robot.jl @@ -1,3 +1,4 @@ +using ADTypes using ModelingToolkit using Multibody using Test @@ -285,7 +286,7 @@ end ssys.axis2.motor.Jmotor.w => 0 ssys.axis3.motor.Jmotor.w => 0 ], (0.0, 2.0)) - @time "simulation (solve)" sol = solve(prob, Rodas5P(autodiff=false)); + @time "simulation (solve)" sol = solve(prob, Rodas5P(autodiff=AutoFiniteDiff())); @test SciMLBase.successful_retcode(sol) end @@ -345,7 +346,7 @@ end # robot.axis2.motor.Jmotor.phi => deg2rad(20) * (210) # robot.axis3.motor.Jmotor.phi => deg2rad(90) * (60) # ], (0.0, 2.0)) -# sol2 = solve(prob, Rodas5P(autodiff=false)) +# sol2 = solve(prob, Rodas5P(autodiff=AutoFiniteDiff())) # @test SciMLBase.successful_retcode(sol2) @@ -354,7 +355,7 @@ end # # @test maximum(abs, control_error) < 0.002 # # using BenchmarkTools -# # @btime solve(prob, Rodas5P(autodiff=false)); +# # @btime solve(prob, Rodas5P(autodiff=AutoFiniteDiff())); # # 152.225 ms (2272926 allocations: 40.08 MiB) # # 114.113 ms (1833534 allocations: 33.38 MiB) # sub 0, 1 # end diff --git a/test/test_wheels.jl b/test/test_wheels.jl index 9d570e8d..56d95fe6 100644 --- a/test/test_wheels.jl +++ b/test/test_wheels.jl @@ -1,4 +1,4 @@ -using TestEnv; TestEnv.activate() +using ADTypes using Test, ModelingToolkit, Multibody, Random using OrdinaryDiffEq t = Multibody.t @@ -96,7 +96,7 @@ defs = Dict([ ]) prob = ODEProblem(ssys, defs, (0, 4); missing_guess_value = MissingGuessValue.Random(Random.GLOBAL_RNG)) -sol = solve(prob, FBDF(autodiff=false), abstol=1e-8, reltol=1e-8) +sol = solve(prob, FBDF(autodiff=AutoFiniteDiff()), abstol=1e-8, reltol=1e-8) @test SciMLBase.successful_retcode(sol) # first(Multibody.render(worldwheel, sol, 0, show_axis=true)) @test sol(4, idxs=[ssys.wheel.x; ssys.wheel.z]) ≈ [0.162547, -2.23778] atol=1e-3 @@ -115,7 +115,7 @@ defs = Dict([ ]) prob = ODEProblem(ssys, defs, (0, 4)) -sol = solve(prob, FBDF(autodiff=false), abstol=1e-8, reltol=1e-8) +sol = solve(prob, FBDF(autodiff=AutoFiniteDiff()), abstol=1e-8, reltol=1e-8) # plot(sol) tv = 0:0.5:4 @test sol(tv, idxs=ssys.wheel.body.r_0[1]) ≈ sol(tv, idxs=ssys.wheel.body.r_0[2]) .- 0.3*sqrt(2) rtol=1e-6 # The sqrt(2) is to account for the shifted contact point at a 45 degree plane @@ -169,7 +169,7 @@ end ssys = multibody(model) prob = ODEProblem(ssys, [], (0, 4), guesses=Dict([ssys.spin_axis.phi => 0.0]); missing_guess_value = MissingGuessValue.Random(Random.GLOBAL_RNG)) @test_skip begin # Singular linear system - sol = solve(prob, Rodas4(autodiff=false), abstol=1e-8, reltol=1e-8) + sol = solve(prob, Rodas4(autodiff=AutoFiniteDiff()), abstol=1e-8, reltol=1e-8) @test_broken !all(iszero, sol.u) end # first(Multibody.render(model, sol, 0, show_axis=true))