From fffb39da8c2da72d42fd62c465d7540c121c0a82 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 3 Oct 2024 06:31:53 +0200 Subject: [PATCH] Multibody validity check (#168) * add multibody check function * use multibody check function * better world handling in examples and tests * remove hacky `W` world constructor * add support for planar mechanics in `multibody` * handle components without gui metadata --- docs/src/examples/free_motion.md | 4 +- docs/src/examples/gearbox.md | 2 +- docs/src/examples/gyroscopic_effects.md | 2 +- docs/src/examples/kinematic_loops.md | 6 +- docs/src/examples/pendulum.md | 33 ++++--- docs/src/examples/prescribed_pose.md | 6 +- docs/src/examples/quad.md | 6 +- docs/src/examples/robot.md | 2 +- docs/src/examples/ropes_and_cables.md | 6 +- docs/src/examples/sensors.md | 2 +- docs/src/examples/space.md | 5 +- docs/src/examples/spherical_pendulum.md | 2 +- docs/src/examples/spring_damper_system.md | 2 +- docs/src/examples/spring_mass_system.md | 2 +- docs/src/examples/suspension.md | 13 ++- docs/src/examples/swing.md | 10 +- docs/src/examples/three_springs.md | 2 +- docs/src/examples/wheel.md | 23 +++-- docs/src/index.md | 5 +- examples/JuliaSim_logo.jl | 3 +- ext/Render.jl | 11 +-- ext/URDF.jl | 5 +- src/Multibody.jl | 38 ++++++++ src/components.jl | 14 ++- src/robot/FullRobot.jl | 1 + src/robot/robot_components.jl | 5 +- test/runtests.jl | 112 +++++++++------------- test/test_JointUSR_RRR.jl | 10 +- test/test_fourbar.jl | 4 +- test/test_orientation_getters.jl | 4 +- test/test_urdf.jl | 2 +- test/test_wheels.jl | 8 +- test/test_worldforces.jl | 26 ++--- 33 files changed, 198 insertions(+), 178 deletions(-) diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index 4bd5ac66..3f2ecd6b 100644 --- a/docs/src/examples/free_motion.md +++ b/docs/src/examples/free_motion.md @@ -22,7 +22,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) systems = [world; freeMotion; body]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0, 10)) @@ -69,7 +69,7 @@ eqs = [connect(bar2.frame_a, world.frame_b) spring2, ]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ collect(body.body.v_0 .=> 0); collect(body.body.w_a .=> 0); diff --git a/docs/src/examples/gearbox.md b/docs/src/examples/gearbox.md index becacf2a..3d1f0aeb 100644 --- a/docs/src/examples/gearbox.md +++ b/docs/src/examples/gearbox.md @@ -48,7 +48,7 @@ eqs = [connect(world.frame_b, gearConstraint.bearing) @named model = ODESystem(eqs, t, systems = [world; systems]) cm = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ D(cm.idealGear.phi_b) => 0 cm.idealGear.phi_b => 0 diff --git a/docs/src/examples/gyroscopic_effects.md b/docs/src/examples/gyroscopic_effects.md index 233db05a..b3b93818 100644 --- a/docs/src/examples/gyroscopic_effects.md +++ b/docs/src/examples/gyroscopic_effects.md @@ -38,7 +38,7 @@ connections = [ @named model = ODESystem(connections, t, systems = [world; systems]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.world.g => 9.80665, model.revolute.w => 10], (0, 5)) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index cfaa4f05..29f34abd 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -75,7 +75,7 @@ connections = [ ] @named fourbar = ODESystem(connections, t, systems = [world; systems]) fourbar = complete(fourbar) -ssys = structural_simplify(IRSystem(fourbar)) +ssys = structural_simplify(multibody(fourbar)) prob = ODEProblem(ssys, [fourbar.j1.phi => 0.1], (0.0, 10.0)) sol = solve(prob, FBDF(autodiff=true)) @@ -146,7 +146,7 @@ connections = [connect(j2.frame_b, b2.frame_a) ] @named fourbar2 = ODESystem(connections, t, systems = [world; systems]) fourbar2 = complete(fourbar2) -ssys = structural_simplify(IRSystem(fourbar2)) +ssys = structural_simplify(multibody(fourbar2)) prob = ODEProblem(ssys, [], (0.0, 1.4399)) # The end time is chosen to make the animation below appear to loop forever @@ -187,7 +187,7 @@ connections = [connect(j2.frame_b, b2.frame_a) @named fourbar_analytic = ODESystem(connections, t, systems = [world; systems]) fourbar_analytic = complete(fourbar_analytic) -ssys_analytic = structural_simplify(IRSystem(fourbar_analytic)) +ssys_analytic = structural_simplify(multibody(fourbar_analytic)) prob = ODEProblem(ssys_analytic, [], (0.0, 1.4399)) sol2 = solve(prob, FBDF(autodiff=true)) # about 4x faster than the simulation above plot!(sol2, idxs=[j2.s]) # Plot the same coordinate as above diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 484bf834..788608aa 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -1,5 +1,5 @@ # Pendulum--The "Hello World of multi-body dynamics" -This beginners tutorial will start by modeling a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world, i.e., the `world` component must be included in all models. +This beginners tutorial will start by modeling a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world, i.e., the `world` component must be included exactly once in all models, at the top level. To start, we load the required packages ```@example pendulum @@ -47,12 +47,16 @@ nothing # hide ``` The `ODESystem` is the fundamental model type in ModelingToolkit used for multibody-type models. -Before we can simulate the system, we must perform model compilation using [`structural_simplify`](@ref) +Before we can simulate the system, we must perform model check using the function [`multibody`](@ref) and compilation using [`structural_simplify`](@ref) ```@example pendulum -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) ``` This results in a simplified model with the minimum required variables and equations to be able to simulate the system efficiently. This step rewrites all `connect` statements into the appropriate equations, and removes any redundant variables and equations. To simulate the pendulum, we require two state variables, one for angle and one for angular velocity, we can see above that these state variables have indeed been chosen. +```@docs +multibody +``` + We are now ready to create an `ODEProblem` and simulate it. We use the `Rodas4` solver from OrdinaryDiffEq.jl, and pass a dictionary for the initial conditions. We specify only initial condition for some variables, for those variables where no initial condition is specified, the default initial condition defined the model will be used. ```@example pendulum D = Differential(t) @@ -90,7 +94,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, damper]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [damper.phi_rel => 1], (0, 10)) @@ -123,7 +127,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body_0, damper, spring]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0, 10)) @@ -150,7 +154,7 @@ connections = [connect(world.frame_b, multibody_spring.frame_a) @named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = Dict(collect(root_body.r_0) .=> [0, 1e-3, 0]) # The spring has a singularity at zero length, so we start some distance away @@ -168,7 +172,7 @@ push!(connections, connect(multibody_spring.spring2d.flange_b, damper.flange_b)) @named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body, damper]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, defs, (0, 10)) sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.()) @@ -186,11 +190,10 @@ This pendulum, sometimes referred to as a _rotary pendulum_, has two joints, one using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq, Plots import ModelingToolkitStandardLibrary.Mechanical.Rotational.Damper as RDamper import Multibody.Rotations -W(args...; kwargs...) = Multibody.world @mtkmodel FurutaPendulum begin @components begin - world = W() + world = World() shoulder_joint = Revolute(n = [0, 1, 0], axisflange = true) elbow_joint = Revolute(n = [0, 0, 1], axisflange = true, phi0=0.1) upper_arm = BodyShape(; m = 0.1, r = [0, 0, 0.6], radius=0.04) @@ -218,7 +221,7 @@ end @named model = FurutaPendulum() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.shoulder_joint.phi => 0.0, model.elbow_joint.phi => 0.1], (0, 10)) sol = solve(prob, Rodas4()) @@ -380,7 +383,7 @@ end end @named model = CartWithInput() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.cartpole.prismatic.s => 0.0, model.cartpole.revolute.phi => 0.1], (0, 10)) sol = solve(prob, Tsit5()) plot(sol, layout=4) @@ -411,13 +414,13 @@ op = Dict([ # Operating point to linearize in cp.revolute.phi => 0 # Pendulum pointing upwards ] ) -matrices, simplified_sys = linearize(IRSystem(cp), inputs, outputs; op) +matrices, simplified_sys = linearize(multibody(cp), inputs, outputs; op) matrices ``` This gives us the matrices $A,B,C,D$ in a linearized statespace representation of the system. To make these easier to work with, we load the control packages and call `named_ss` instead of `linearize` to get a named statespace object instead: ```@example pendulum using ControlSystemsMTK -lsys = named_ss(IRSystem(cp), inputs, outputs; op) # identical to linearize, but packages the resulting matrices in a named statespace object for convenience +lsys = named_ss(multibody(cp), inputs, outputs; op) # identical to linearize, but packages the resulting matrices in a named statespace object for convenience ``` ### LQR Control design @@ -466,7 +469,7 @@ LQGSystem(args...; kwargs...) = ODESystem(observer_controller(lqg); kwargs...) end @named model = CartWithFeedback() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.cartpole.prismatic.s => 0.1, model.cartpole.revolute.phi => 0.35], (0, 10)) sol = solve(prob, Tsit5()) cp = model.cartpole @@ -533,7 +536,7 @@ normalize_angle(x::Number) = mod(x+3.1415, 2pi)-3.1415 end @named model = CartWithSwingup() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) cp = model.cartpole prob = ODEProblem(ssys, [cp.prismatic.s => 0.0, cp.revolute.phi => 0.99pi], (0, 5)) sol = solve(prob, Tsit5(), dt = 1e-2, adaptive=false) diff --git a/docs/src/examples/prescribed_pose.md b/docs/src/examples/prescribed_pose.md index ccd5c871..2cd3f0c6 100644 --- a/docs/src/examples/prescribed_pose.md +++ b/docs/src/examples/prescribed_pose.md @@ -30,7 +30,7 @@ using Test t = Multibody.t D = Differential(t) -W(args...; kwargs...) = Multibody.world + n = [1, 0, 0] AB = 146.5 / 1000 @@ -131,7 +131,7 @@ end rod_radius = 0.02 end @components begin - world = W() + world = World() mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)])) excited_suspension = ExcitedWheelAssembly(; rod_radius) prescribed_motion = Pose(; r = [0, 0.1 + 0.1sin(t), 0], R = RotXYZ(0, 0.5sin(t), 0)) @@ -144,7 +144,7 @@ end @named model = SuspensionWithExcitationAndMass() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) display([unknowns(ssys) diag(ssys.mass_matrix)]) defs = [ diff --git a/docs/src/examples/quad.md b/docs/src/examples/quad.md index e4f5ad1b..ff1d60a6 100644 --- a/docs/src/examples/quad.md +++ b/docs/src/examples/quad.md @@ -191,7 +191,7 @@ function RotorCraft(; closed_loop = true, addload=true, L=nothing, outputs = not end model = RotorCraft(closed_loop=true, addload=true, pid=true) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) # display(unknowns(ssys)) op = [ model.body.v_0[1] => 0; @@ -234,7 +234,7 @@ op = [ inputs .=> 1; ] |> Dict -@time lsys = named_ss(IRSystem(quad), inputs, outputs; op) +@time lsys = named_ss(multibody(quad), inputs, outputs; op) rsys = minreal(sminreal(lsys)) C = rsys.C rank(C) >= rsys.nx || @warn "The output matrix C is not full rank" @@ -265,7 +265,7 @@ L ModelingToolkit.get_iv(i::IRSystem) = i.t model = RotorCraft(; closed_loop=true, addload=true, L=-L, outputs) # Negate L for negative feedback model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) op = [ model.body.r_0[2] => 1e-3 diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index eae0affd..608f229c 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -21,7 +21,7 @@ The robot is a medium sized system with some 2000 equations before simplificatio After simplification, the following states are chosen: ```@example robot -ssys = structural_simplify(IRSystem(robot)) +ssys = structural_simplify(multibody(robot)) unknowns(ssys) ``` diff --git a/docs/src/examples/ropes_and_cables.md b/docs/src/examples/ropes_and_cables.md index 93e81712..db5a8c77 100644 --- a/docs/src/examples/ropes_and_cables.md +++ b/docs/src/examples/ropes_and_cables.md @@ -31,7 +31,7 @@ connections = [connect(world.frame_b, rope.frame_a) @named stiff_rope = ODESystem(connections, t, systems = [world, body, rope]) stiff_rope = complete(stiff_rope) -ssys = structural_simplify(IRSystem(stiff_rope)) +ssys = structural_simplify(multibody(stiff_rope)) prob = ODEProblem(ssys, [], (0, 5)) sol = solve(prob, Rodas4(autodiff=false)) @test SciMLBase.successful_retcode(sol) @@ -55,7 +55,7 @@ connections = [connect(world.frame_b, rope.frame_a) @named flexible_rope = ODESystem(connections, t, systems = [world, body, rope]) flexible_rope = complete(flexible_rope) -ssys = structural_simplify(IRSystem(flexible_rope)) +ssys = structural_simplify(multibody(flexible_rope)) prob = ODEProblem(ssys, [], (0, 8)) sol = solve(prob, Rodas4(autodiff=false)); @test SciMLBase.successful_retcode(sol) @@ -88,7 +88,7 @@ connections = [connect(world.frame_b, fixed.frame_a, chain.frame_a) @named mounted_chain = ODESystem(connections, t, systems = [systems; world]) mounted_chain = complete(mounted_chain) -ssys = structural_simplify(IRSystem(mounted_chain)) +ssys = structural_simplify(multibody(mounted_chain)) prob = ODEProblem(ssys, [ collect(chain.link_8.body.w_a) .=> [0,0,0]; collect(chain.link_8.frame_b.r_0) .=> [x_dist,0,0]; diff --git a/docs/src/examples/sensors.md b/docs/src/examples/sensors.md index 2cc8d15d..2bcb730f 100644 --- a/docs/src/examples/sensors.md +++ b/docs/src/examples/sensors.md @@ -31,7 +31,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, torquesensor, forcesensor]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) D = Differential(t) diff --git a/docs/src/examples/space.md b/docs/src/examples/space.md index 314baf99..71d822da 100644 --- a/docs/src/examples/space.md +++ b/docs/src/examples/space.md @@ -23,11 +23,10 @@ using OrdinaryDiffEq t = Multibody.t D = Differential(t) -W(;kwargs...) = Multibody.world @mtkmodel PointGrav begin @components begin - world = W() + world = World() body1 = Body( m=1, I_11=0.1, @@ -50,7 +49,7 @@ W(;kwargs...) = Multibody.world end @named model = PointGrav() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = [ model.world.mu => 1 model.world.point_gravity => true # The gravity model is selected here diff --git a/docs/src/examples/spherical_pendulum.md b/docs/src/examples/spherical_pendulum.md index d1f97675..4ae108e5 100644 --- a/docs/src/examples/spherical_pendulum.md +++ b/docs/src/examples/spherical_pendulum.md @@ -28,7 +28,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world; systems]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0, 5)) diff --git a/docs/src/examples/spring_damper_system.md b/docs/src/examples/spring_damper_system.md index db149731..fa7375ce 100644 --- a/docs/src/examples/spring_damper_system.md +++ b/docs/src/examples/spring_damper_system.md @@ -53,7 +53,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) damper1, ]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ damper1.d => 2; diff --git a/docs/src/examples/spring_mass_system.md b/docs/src/examples/spring_mass_system.md index e39238ff..b5ea518a 100644 --- a/docs/src/examples/spring_mass_system.md +++ b/docs/src/examples/spring_mass_system.md @@ -45,7 +45,7 @@ eqs = [ ] @named model = ODESystem(eqs, t, systems = [world; systems]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys,[], (0, 5)) sol = solve(prob, Rodas4()) diff --git a/docs/src/examples/suspension.md b/docs/src/examples/suspension.md index d39b66c2..20ef81fa 100644 --- a/docs/src/examples/suspension.md +++ b/docs/src/examples/suspension.md @@ -18,7 +18,6 @@ using Test t = Multibody.t D = Differential(t) -W(args...; kwargs...) = Multibody.world n = [1, 0, 0] AB = 146.5 / 1000 @@ -137,7 +136,7 @@ end dir = mirror ? -1 : 1 end @components begin - world = W() + world = World() mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)*dir])) excited_suspension = SuspensionWithExcitation(; suspension.spring=true, mirror, rod_radius) body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=1000) @@ -151,7 +150,7 @@ end @named model = SuspensionWithExcitationAndMass() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = [ model.body_upright.s => 0.17 @@ -230,7 +229,7 @@ end @named model = DoubleSuspensionWithExcitationAndMass() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = [ model.excited_suspension_r.amplitude => 0.05 @@ -343,7 +342,7 @@ end @named model = SuspensionWithExcitationAndMass() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) display([unknowns(ssys) diag(ssys.mass_matrix)]) defs = [ @@ -398,7 +397,7 @@ end @named model = HalfCar() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = [ model.excited_suspension_r.amplitude => 0.015 @@ -477,7 +476,7 @@ end @named model = FullCar() model = complete(model) -@time "simplification" ssys = structural_simplify(IRSystem(model)) +@time "simplification" ssys = structural_simplify(multibody(model)) defs = [ model.excited_suspension_br.wheel.wheeljoint.v_small => 1e-3 diff --git a/docs/src/examples/swing.md b/docs/src/examples/swing.md index a6b39926..939672a2 100644 --- a/docs/src/examples/swing.md +++ b/docs/src/examples/swing.md @@ -18,8 +18,6 @@ t = Multibody.t D = Differential(t) world = Multibody.world -W(args...; kwargs...) = Multibody.world - @mtkmodel SwingRope begin @components begin frame_a = Frame() @@ -44,7 +42,7 @@ end w = 0.4 end @components begin - world = W() + world = World() upper_trans1 = FixedTranslation(r=[-w/2, 0, 0]) rope1 = SwingRope(rope.r=[-w/2, h, -w/2]) body = Body(m=6, isroot=true, I_11=0.1, I_22=0.1, I_33=0.1) @@ -62,7 +60,7 @@ end end @named model = SimpleSwing() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ collect(model.body.v_0) .=> 0; collect(model.body.w_a) .=> 0; @@ -91,7 +89,7 @@ Next, we create the full swing assembly w = 0.4 end @components begin - world = W() + 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]) @@ -129,7 +127,7 @@ end @named model = Swing() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) d = 10 dj = 0.01 diff --git a/docs/src/examples/three_springs.md b/docs/src/examples/three_springs.md index 27288269..31909eff 100644 --- a/docs/src/examples/three_springs.md +++ b/docs/src/examples/three_springs.md @@ -37,7 +37,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) connect(spring2.frame_a, spring1.frame_b)] @named model = ODESystem(eqs, t, systems = [world; systems]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0, 10)) sol = solve(prob, Rodas4()) diff --git a/docs/src/examples/wheel.md b/docs/src/examples/wheel.md index 898b3f95..d43ef4ab 100644 --- a/docs/src/examples/wheel.md +++ b/docs/src/examples/wheel.md @@ -31,11 +31,10 @@ using Test t = Multibody.t D = Differential(t) -W(args...; kwargs...) = Multibody.world @mtkmodel WheelInWorld begin @components begin - world = W() + world = World() wheel = RollingWheel( radius = 0.3, m = 2, @@ -57,7 +56,7 @@ defs = Dict([ worldwheel.wheel.body.r_0[3] => 0.2; ]) -ssys = structural_simplify(IRSystem(worldwheel)) +ssys = structural_simplify(multibody(worldwheel)) prob = ODEProblem(ssys, defs, (0, 4)) sol = solve(prob, Tsit5()) @test SciMLBase.successful_retcode(sol) @@ -104,7 +103,7 @@ The slip velocity is defined such that when the wheel is moving with positive ve ```@example WHEEL @mtkmodel SlipWheelInWorld begin @components begin - world = W() + world = World() wheel = SlippingWheel( radius = 0.3, m = 2, @@ -135,7 +134,7 @@ defs = Dict([ worldwheel.wheel.frame_a.radius => 0.01; ]) -ssys = structural_simplify(IRSystem(worldwheel)) +ssys = structural_simplify(multibody(worldwheel)) prob = ODEProblem(ssys, defs, (0, 3)) sol = solve(prob, Tsit5()) @test SciMLBase.successful_retcode(sol) @@ -164,7 +163,7 @@ A [`RollingWheelSet`](@ref) is comprised out of two wheels mounted on a common a wheels = RollingWheelSet(radius=0.1, m_wheel=0.5, I_axis=0.01, I_long=0.02, track=0.5, state_priority=100) bar = FixedTranslation(r = [0.2, 0, 0]) body = Body(m=0.01, state_priority=1) - world = W() + world = World() end @equations begin connect(sine1.output, torque1.tau) @@ -178,7 +177,7 @@ end @named model = DrivingWheelSet() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) # display(unknowns(ssys)) prob = ODEProblem(ssys, [ model.wheels.wheelSetJoint.prismatic1.s => 0.1 @@ -216,7 +215,7 @@ tire_black = [0.1, 0.1, 0.1, 1] g=0 end @components begin - world = W() + world = World() sine1 = Blocks.Sine(frequency=1, amplitude=150) sine2 = Blocks.Sine(frequency=1, amplitude=150, phase=pi/6) @@ -246,7 +245,7 @@ tire_black = [0.1, 0.1, 0.1, 1] end @named model = Car() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0, 6)) sol = solve(prob, Tsit5()) @@ -286,7 +285,7 @@ import Multibody.PlanarMechanics as Pl end @named model = TestWheel() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = Dict(unknowns(ssys) .=> 0) prob = ODEProblem(ssys, defs, (0.0, 10.0)) sol = solve(prob, Rodas5P()) @@ -352,7 +351,7 @@ end @named model = TestSlipBasedWheel() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = ModelingToolkit.defaults(model) prob = ODEProblem(ssys, [ model.inertia.w => 1e-10, # This is important, at zero velocity, the friction is ill-defined @@ -482,7 +481,7 @@ end @named model = TwoTrackWithDifferentialGear() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = merge( Dict(unknowns(ssys) .=> 0), ModelingToolkit.defaults(model), diff --git a/docs/src/index.md b/docs/src/index.md index 3d971042..616ca538 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -12,7 +12,6 @@ using Multibody, JuliaSimCompiler using OrdinaryDiffEq # Contains the ODE solver we will use using Plots t = Multibody.t -W(args...; kwargs...) = Multibody.world JULIASIM_PURPLE = [87,87,219,255.0f0]./255 # RGBA @@ -60,10 +59,10 @@ end @named logo = Logo() logo = complete(logo) -ssys = structural_simplify(IRSystem(logo)) +ssys = structural_simplify(multibody(logo)) prob = ODEProblem(ssys, [], (0.0, 3.51)) sol = solve(prob, Rodas5P()) -Plots.plot(sol) +# Plots.plot(sol) import GLMakie framerate = 30 diff --git a/examples/JuliaSim_logo.jl b/examples/JuliaSim_logo.jl index 81ac801c..4aa7dd58 100644 --- a/examples/JuliaSim_logo.jl +++ b/examples/JuliaSim_logo.jl @@ -3,7 +3,6 @@ using Multibody, JuliaSimCompiler using OrdinaryDiffEq # Contains the ODE solver we will use using Plots t = Multibody.t -W(args...; kwargs...) = Multibody.world JULIASIM_PURPLE = [87,87,219,255.0f0]./255 # RGBA @@ -51,7 +50,7 @@ end @named logo = Logo() logo = complete(logo) -ssys = structural_simplify(IRSystem(logo)) +ssys = structural_simplify(multibody(logo)) prob = ODEProblem(ssys, [], (0.0, 3.51)) sol = solve(prob, Rodas5P()) Plots.plot(sol) diff --git a/ext/Render.jl b/ext/Render.jl index c2587891..416b8fcb 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -1,7 +1,7 @@ module Render using Makie using Multibody -import Multibody: render, render!, loop_render, encode, decode, get_rot, get_trans, get_frame +import Multibody: render, render!, loop_render, encode, decode, get_rot, get_trans, get_frame, get_systemtype import Multibody.PlanarMechanics as P using Rotations using LinearAlgebra @@ -205,13 +205,6 @@ function get_frame_fun(sol, frame) end -"get_systemtype(sys): Get the constructor of a component for dispatch purposes. This only supports components that have the `gui_metadata` property set. If no metadata is available, nothing is returned." -function get_systemtype(sys) - meta = getfield(sys, :gui_metadata) - meta === nothing && return nothing - eval(meta.type) -end - function get_color(sys, sol, default, var_name = :color) try Makie.RGBA(sol(sol.t[1], idxs=collect(getproperty(sys, var_name)))...) @@ -786,7 +779,7 @@ function render!(scene, ::Union{typeof(RollingWheelJoint), typeof(SlipWheelJoint width = radius/10 p1 = Point3f(O + width*n_w) p2 = Point3f(O - width*n_w) - Makie.GeometryBasics.Cylinder(p1, p2, radius) + Makie.normal_mesh(Makie.Tesselation(Makie.GeometryBasics.Cylinder(p1, p2, radius), 64)) end mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1)) true diff --git a/ext/URDF.jl b/ext/URDF.jl index a59d59fb..a6465912 100644 --- a/ext/URDF.jl +++ b/ext/URDF.jl @@ -310,7 +310,6 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no """ using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq, Plots import ModelingToolkit: t_nounits as t, D_nounits as D - W(args...; kwargs...) = Multibody.world """ else "" @@ -318,7 +317,7 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no s = s * """ @mtkmodel $(modelname) begin @components begin - world = W() + world = World() $(join(bodies, "\n")) $(join(joints, "\n")) end @@ -332,7 +331,7 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no s = s * """ @named model = $(modelname)() model = complete(model) - ssys = structural_simplify(IRSystem(model)) + ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0.0, 10.0)) sol = solve(prob, $(solver)()) plot(sol) |> display diff --git a/src/Multibody.jl b/src/Multibody.jl index 56c8effd..ee0a686a 100644 --- a/src/Multibody.jl +++ b/src/Multibody.jl @@ -60,6 +60,44 @@ function benchmark_f(prob) @eval Main @btime $(prob.f)($dx, $x, $p, 0.0) end +"get_systemtype(sys): Get the constructor of a component for dispatch purposes. This only supports components that have the `gui_metadata` property set. If no metadata is available, nothing is returned." +function get_systemtype(sys) + meta = getfield(sys, :gui_metadata) + meta === nothing && return nothing + eval(meta.type) +end + +export multibody + +""" + multibody(model) + +Perform validity checks on the model, such as the precense of exactly one world component in the top level of the model, and transform the model into an `IRSystem` object for passing into `structural_simplify`. +""" +function multibody(model, level=0) + found_world = false + found_planar = false + for subsys in model.systems + system_type = get_systemtype(subsys) + subsys_ns = getproperty(model, subsys.name) + isworld = system_type == World + isplanar = system_type !== nothing && parentmodule(system_type) == PlanarMechanics + found_world = found_world || isworld + found_planar = found_planar || isplanar + multibody(subsys_ns, level + 1) + end + if level == 0 && !found_world && !found_planar + @warn("No world found in the top level of the model, this may lead to missing equations") + elseif level != 0 && found_world + @warn("World found in a non-top level component ($(nameof(model))) of the model, this may lead to extra equations. Consider using the component `Fixed` instead of `World` in component models.") + end + if level == 0 + return IRSystem(model) + else + return nothing + end +end + """ scene = render(model, prob) scene, time = render(model, sol, t::Real; framerate = 30, traces = []) diff --git a/src/components.jl b/src/components.jl index 65f761e9..b3609eba 100644 --- a/src/components.jl +++ b/src/components.jl @@ -36,7 +36,19 @@ function ori(sys, varw = false) end """ - World(; name, render=true) + World(; name, render=true, point_gravity=false, n = [0.0, -1.0, 0.0], g=9.80665, mu=3.986004418e14) + +All multibody models must include exactly one world component defined at the top level. The `frame_b` of the world is fixed in the origin. + +If a connection to the world is needed in a component model, use [`Fixed`](@ref) instead. + +# Arguments +- `name`: Name of the world component +- `render`: Render the component in animations +- `point_gravity`: If `true`, the gravity is always opinting towards a single point in space. If `false`, the gravity is always pointing in the same direction `n`. +- `n`: Gravity direction unit vector, defaults to [0, -1, 0], only applicable if `point_gravity = false` +- `g`: Gravitational acceleration, defaults to 9.80665 +- `mu`: Gravity field constant, defaults to 3.986004418e14, only applicable to point gravity """ @component function World(; name, render=true, point_gravity=false, n = [0.0, -1.0, 0.0], g=9.80665, mu=3.986004418e14) # World should have diff --git a/src/robot/FullRobot.jl b/src/robot/FullRobot.jl index a5be98a8..983f48de 100644 --- a/src/robot/FullRobot.jl +++ b/src/robot/FullRobot.jl @@ -158,6 +158,7 @@ function Robot6DOF(; name, kwargs...) q16 = 45 # Can't yet have these as parameters systems = @named begin + world = World() mechanics = MechanicalStructure(mLoad = (mLoad), rLoad = (rLoad), g = (g)) diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 84b29b3e..c0751a89 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -407,6 +407,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) path = @__DIR__() systems = @named begin + fixed = Fixed() axis1 = Rotational.Flange() axis2 = Rotational.Flange() axis3 = Rotational.Flange() @@ -525,7 +526,7 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) qdd .~ D.(qd) tau .~ [r1.tau, r2.tau, r3.tau, r4.tau, r5.tau, r6.tau] connect(load.frame_a, b6.frame_b) - connect(world.frame_b, b0.frame_a) + connect(fixed.frame_b, b0.frame_a) connect(b0.frame_b, r1.frame_a) connect(b1.frame_b, r2.frame_a) connect(r1.frame_b, b1.frame_a) @@ -545,5 +546,5 @@ function MechanicalStructure(; name, mLoad = 15, rLoad = [0, 0.25, 0], g = 9.81) connect(r6.axis, axis6) connect(r6.frame_b, b6.frame_a)] - compose(ODESystem(eqs, t; name), [world; systems]) + compose(ODESystem(eqs, t; name), systems) end diff --git a/test/runtests.jl b/test/runtests.jl index eeadd19d..d89e7d94 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -8,23 +8,7 @@ t = Multibody.t D = Differential(t) doplot() = false world = Multibody.world -W(args...; kwargs...) = Multibody.world -## Only body and world -@named body = Body(; m = 1, isroot = false, r_cm = [1, 0, 1]) -Multibody.isroot(body) - -connections = [ - connect(world.frame_b, body.frame_a), -] - -@named model = ODESystem(connections, t, systems = [world, body]) - -modele = ModelingToolkit.expand_connections(model) - -ssys = structural_simplify(model) - -@test length(unknowns(ssys)) == 0 # This example is completely rigid and should simplify down to zero state variables @testset "world" begin @info "Testing world" @@ -93,7 +77,7 @@ D = Differential(t) # ssys = structural_simplify(model, allow_parameter = false) - irsys = IRSystem(model) + irsys = multibody(model) ssys = structural_simplify(irsys) D = Differential(t) @@ -143,10 +127,9 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, torksensor, forcesensor, powersensor, damper]) -modele = ModelingToolkit.expand_connections(model) # ssys = structural_simplify(model, allow_parameter = false) -irsys = IRSystem(modele) +irsys = multibody(model) ssys = structural_simplify(irsys) D = Differential(t) @@ -178,7 +161,7 @@ end # ============================================================================== @mtkmodel PointGrav begin @components begin - world = W() + world = World() body1 = Body( m=1, I_11=0.1, @@ -199,7 +182,7 @@ end end @named model = PointGrav() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = [ model.world.mu => 1 model.world.point_gravity => true @@ -230,10 +213,9 @@ connections = [connect(world.frame_b, rev.frame_a) connect(body.frame_a, rev.frame_b)] @named model = ODESystem(connections, t, systems = [world, rev, body, damper]) -modele = ModelingToolkit.expand_connections(model) # ssys = structural_simplify(model, allow_parameter = false) -irsys = IRSystem(modele) +irsys = multibody(model) ssys = structural_simplify(irsys) D = Differential(t) prob = ODEProblem(ssys, [damper.phi_rel => 1, D(rev.phi) => 0, D(D(rev.phi)) => 0], @@ -269,7 +251,7 @@ connections = [connect(world.frame_b, rev.frame_a) # modele = ModelingToolkit.expand_connections(model) # ssys = structural_simplify(model, allow_parameter = false) -ssys = structural_simplify(IRSystem(model))#, alias_eliminate = false) +ssys = structural_simplify(multibody(model))#, alias_eliminate = false) D = Differential(t) prob = ODEProblem(ssys, [damper.phi_rel => 1, D(rev.phi) => 0, D(D(rev.phi)) => 0], @@ -300,10 +282,9 @@ connections = [connect(world.frame_b, rev.frame_a) connect(rod.frame_b, body.frame_a)] @named model = ODESystem(connections, t, systems = [world, rev, body, damper, rod]) -modele = ModelingToolkit.expand_connections(model) # ssys = structural_simplify(model)#, allow_parameter = false) -ssys = structural_simplify(IRSystem(modele)) +ssys = structural_simplify(multibody(model)) D = Differential(t) @@ -356,7 +337,7 @@ connections = [connect(damper1.flange_b, rev1.axis) # modele = ModelingToolkit.expand_connections(model) # ssys = structural_simplify(model, allow_parameter = false) -irsys = IRSystem(model) +irsys = multibody(model) ssys = structural_simplify(irsys, alias_eliminate = false) D = Differential(t) prob = ODEProblem(ssys, @@ -391,7 +372,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(body.frame_a, joint.frame_b)] @named model = ODESystem(connections, t, systems = [world, joint, body, damper, spring]) -ssys = structural_simplify(IRSystem(model))#, allow_parameter = false) +ssys = structural_simplify(multibody(model))#, allow_parameter = false) prob = ODEProblem(ssys, [damper.s_rel => 1, D(joint.s) => 0, D(D(joint.s)) => 0], (0, 30)) @@ -407,7 +388,7 @@ end @testset "Spring damper system" begin systems = @named begin - world = W() + world = World() body1 = Body(; m = 1, isroot = true, r_cm = [0.0, 0, 0], I_11 = 0.1, I_22 = 0.1, I_33 = 0.1, r_0 = [0.3, -0.2, 0], quat=false) # This is root since there is no joint parallel to the spring leading to this body body2 = Body(; m = 1, isroot = false, r_cm = [0.0, -0.2, 0]) # This is not root since there is a joint parallel to the spring leading to this body @@ -440,7 +421,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) @named model = ODESystem(eqs, t; systems) # ssys = structural_simplify(model, allow_parameter = false) -ssys = structural_simplify(IRSystem(model))#, alias_eliminate = false) +ssys = structural_simplify(multibody(model))#, alias_eliminate = false) prob = ODEProblem(ssys, [#collect(D.(body1.phid)) .=> 0; @@ -519,7 +500,7 @@ eqs = [connect(world.frame_b, bar1.frame_a) spring2, spring3, ]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) # ssys = structural_simplify(model, allow_parameters = false) prob = ODEProblem(ssys, [ collect(body1.v_0 .=> 0); @@ -562,7 +543,7 @@ eqs = [connect(bar2.frame_a, world.frame_b) spring1, spring2, ]) -ssys = structural_simplify(IRSystem(model))#, alias_eliminate = true) +ssys = structural_simplify(multibody(model))#, alias_eliminate = true) # ssys = structural_simplify(model, allow_parameters = false) prob = ODEProblem(ssys, [collect(body.body.w_a .=> 0); @@ -584,7 +565,7 @@ end using LinearAlgebra @mtkmodel PlanarTest begin @components begin - world = W() + world = World() planar = Planar(n=[0,0,1], n_x=[1,0,0]) force = Force() body = Body(m=1) @@ -600,7 +581,7 @@ using LinearAlgebra end @named sys = PlanarTest() sys = complete(sys) -ssys = structural_simplify(IRSystem(sys)) +ssys = structural_simplify(multibody(sys)) prob = ODEProblem(ssys, [ sys.world.g => 9.80665; # Modelica default # collect(sys.body.w_a) .=> 0; @@ -641,7 +622,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, bar, body]) # ssys = structural_simplify(model, allow_parameters = false) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) @test length(unknowns(ssys)) == 6 @@ -682,7 +663,7 @@ connections = [connect(world.frame_b, joint.frame_a) connect(bar.frame_b, body.frame_a)] @named model = ODESystem(connections, t, systems = [world, joint, bar, body]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ @@ -744,7 +725,7 @@ eqs = [connect(world.frame_b, gearConstraint.bearing) @named model = ODESystem(eqs, t, systems = [world; systems]) cm = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ cm.idealGear.phi_b => 0 D(cm.idealGear.phi_b) => 0 @@ -779,7 +760,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) freeMotion; body]) # ssys = structural_simplify(model, allow_parameters = false) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(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)) @@ -803,7 +784,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) freeMotion; body]) # ssys = structural_simplify(model, allow_parameters = false) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(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)) @@ -822,7 +803,7 @@ world = Multibody.world systems = [world; body]) # ssys = structural_simplify(model, allow_parameters = false) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) @test length(unknowns(ssys)) == 12 prob = ODEProblem(ssys, [ @@ -854,7 +835,7 @@ eqs = [connect(world.frame_b, freeMotion.frame_a) freeMotion; body]) # ssys = structural_simplify(model, allow_parameters = false) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, Symbolics.scalarize.([ @@ -891,11 +872,10 @@ D = Differential(t) # Workarounds for @mtkmodel bugs and limitations RTorque = Rotational.Torque BSine = Blocks.Sine -W(args...; kwargs...) = Multibody.world @mtkmodel ActuatedJoint begin @components begin - world = W() + world = World() torque = RTorque() joint = Revolute(axisflange=true) # The axis flange provides an interface to the 1D torque input from ModelingToolkitStandardLibrary.Mechanical.Rotational torque_signal = BSine(frequency=1/5) @@ -911,7 +891,7 @@ end @named model = ActuatedJoint() cm = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [D(D(cm.joint.phi)) => 0], (0, 10)) sol = solve(prob, Rodas4()) @test SciMLBase.successful_retcode(sol) @@ -940,7 +920,7 @@ connections = [connect(world.frame_b, rope.frame_a) @named stiff_rope = ODESystem(connections, t, systems = [world, body, rope]) # ssys = structural_simplify(model, allow_parameter = false) -@time "Simplify stiff rope pendulum" ssys = structural_simplify(IRSystem(stiff_rope)) +@time "Simplify stiff rope pendulum" ssys = structural_simplify(multibody(stiff_rope)) D = Differential(t) prob = ODEProblem(ssys, [ @@ -970,7 +950,7 @@ connections = [connect(world.frame_b, rope.frame_a) @named flexible_rope = ODESystem(connections, t, systems = [world, body, rope]) # ssys = structural_simplify(model, allow_parameter = false) -@time "Simplify flexible rope pendulum" ssys = structural_simplify(IRSystem(flexible_rope)) +@time "Simplify flexible rope pendulum" ssys = structural_simplify(multibody(flexible_rope)) D = Differential(t) prob = ODEProblem(ssys, [ # D.(D.(collect(rope.r))) .=> 0; @@ -1019,7 +999,7 @@ connections = [connect(world.frame_b, joint.frame_a) ] @named model = ODESystem(connections, t, systems = [world; systems]) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ D.(joint.phi) .=> 0; @@ -1052,7 +1032,7 @@ tt = 0:0.1:10 @named mounted_chain = ODESystem(connections, t, systems = [systems; world]) - ssys = structural_simplify(IRSystem(mounted_chain)) + ssys = structural_simplify(multibody(mounted_chain)) prob = ODEProblem(ssys, [ collect(chain.link_3.body.w_a) .=> [0,0,0]; collect(chain.link_3.frame_b.r_0) .=> [x_dist,0,0]; @@ -1074,7 +1054,7 @@ using LinearAlgebra world = Multibody.world @mtkmodel CylinderPend begin @components begin - world = W() + world = World() body = BodyCylinder(r=[1,2,3], diameter=0.1) joint = Revolute() end @@ -1086,7 +1066,7 @@ using LinearAlgebra @named model = CylinderPend() model = complete(model) - ssys = structural_simplify(IRSystem(model)) + ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.joint.phi => 0], (0, 10)) sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8) @@ -1122,7 +1102,7 @@ using LinearAlgebra @info "Testing BodyBox" @mtkmodel BoxPend begin @components begin - world = W() + world = World() body = Multibody.BodyBox(r=[0.1, 1, 0.2], r_shape=[0, 0, 0], width=0.1, height=0.3, inner_width=0.05) joint = Revolute() end @@ -1134,7 +1114,7 @@ using LinearAlgebra @named model = BoxPend() model = complete(model) - ssys = structural_simplify(IRSystem(model)) + ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.joint.phi => 0], (0, 1)) sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8) @@ -1176,7 +1156,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, rod]) -irsys = IRSystem(model) +irsys = multibody(model) ssys = structural_simplify(irsys) prob = ODEProblem(ssys, [ # vec(ori(rod.frame_a).R) .=> vec(RotXYZ(0,0,0)); @@ -1197,7 +1177,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, rod]) -irsys = IRSystem(model) +irsys = multibody(model) ssys = structural_simplify(irsys) prob = ODEProblem(ssys, [ # vec(ori(rod.frame_a).R) .=> vec(RotXYZ(0,0,0)); @@ -1218,7 +1198,7 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, rod]) -irsys = IRSystem(model) +irsys = multibody(model) ssys = structural_simplify(irsys) prob = ODEProblem(ssys, [ # vec(ori(rod.frame_a).R) .=> vec(RotXYZ(0,0,0)); @@ -1237,7 +1217,7 @@ sol3 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8) @mtkmodel TestSphericalSpherical begin @components begin - world = W() + world = World() ss = SphericalSpherical(r_0 = [1, 0, 0], m = 1, kinematic_constraint=false) ss2 = BodyShape(r = [0, 0, 1], m = 1, isroot=true) s = Spherical() @@ -1253,7 +1233,7 @@ end @named model = TestSphericalSpherical() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ model.ss2.body.phi[1] => 0.1; model.ss2.body.phid[3] => 0.0; @@ -1268,7 +1248,7 @@ sol = solve(prob, Rodas4()) @mtkmodel TestSphericalSpherical begin @components begin - world = W() + world = World() ss = UniversalSpherical(rRod_ia = [1, 0, 0], kinematic_constraint=false, sphere_diameter=0.3) ss2 = BodyShape(r = [0, 0, 1], m = 1, isroot=true) s = Spherical() @@ -1289,7 +1269,7 @@ end @named model = TestSphericalSpherical() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ model.ss2.body.phi[1] => 0.1; model.ss2.body.phi[3] => 0.1; @@ -1311,7 +1291,7 @@ end # Test cylindrical joint @mtkmodel CylinderTest begin @components begin - world = W() + world = World() cyl = Cylindrical(n = [0, 1, 0]) # spring = Spring(c = 1) body = Body(state_priority=0) @@ -1326,7 +1306,7 @@ end end @named model = CylinderTest() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [ model.cyl.revolute.w => 1 ], (0, 1)) @@ -1372,7 +1352,7 @@ import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Transl jr = 0.03, [description = "Radius of revolute joint"] end @components begin - world = W() + world = World() r1 = Revolute(; n, radius=jr, color=jc) r2 = Revolute(; n, radius=jr, color=jc) @@ -1455,7 +1435,7 @@ defs = [ model.r3.phi => 0.47595 ] -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) display(sort(unknowns(ssys), by=string)) ## @@ -1472,7 +1452,7 @@ end @testset "QuarterCar JointRRR" begin @info "Testing QuarterCar JointRRR" -## Quarter car with JointRRR +# Quarter car with JointRRR n = [1, 0, 0] AB = 146.5 / 1000 BC = 233.84 / 1000 @@ -1502,7 +1482,7 @@ import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Transl rRod2_ib = BC*normalize([0, 0.2, 0]) end @components begin - world = W() + world = World() r123 = JointRRR(n_a = n, n_b = n, rRod1_ia, rRod2_ib, rod_radius=0.02, rod_color=jc) r2 = Revolute(; n, radius=jr, color=jc) @@ -1561,7 +1541,7 @@ end @named model = QuarterCarSuspension(spring=true) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) defs = [ vec(ori(model.chassis.body.frame_a).R .=> I(3)) diff --git a/test/test_JointUSR_RRR.jl b/test/test_JointUSR_RRR.jl index 01b68953..fb6c3506 100644 --- a/test/test_JointUSR_RRR.jl +++ b/test/test_JointUSR_RRR.jl @@ -8,7 +8,7 @@ using LinearAlgebra using JuliaSimCompiler world = Multibody.world -W(args...; kwargs...) = Multibody.world + t = Multibody.t D = Differential(t) @@ -17,7 +17,7 @@ D = Differential(t) # ============================================================================== @mtkmodel TestUSR begin @components begin - world = W() + world = World() j1 = JointUSR(positive_branch=true, use_arrays=false) fixed = FixedTranslation(r=[1,0,0]) b1 = Body(m=1) @@ -33,7 +33,7 @@ end @named model = TestUSR() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) @test length(unknowns(ssys)) == 2 ## @@ -48,7 +48,7 @@ sol = solve(prob, FBDF(autodiff=true)) # ============================================================================== @mtkmodel TestRRR begin @components begin - world = W() + world = World() j1 = JointRRR(positive_branch=true) fixed = FixedTranslation(r=[1,0,0]) b1 = Body(m=1) @@ -64,7 +64,7 @@ end @named model = TestRRR() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) @test length(unknowns(ssys)) == 2 ## diff --git a/test/test_fourbar.jl b/test/test_fourbar.jl index a64c7886..169e798d 100644 --- a/test/test_fourbar.jl +++ b/test/test_fourbar.jl @@ -39,7 +39,7 @@ connections = [connect(j2.frame_b, b2.frame_a) ] @named fourbar2 = ODESystem(connections, t, systems = [world; systems]) fourbar2 = complete(fourbar2) -ssys = structural_simplify(IRSystem(fourbar2)) +ssys = structural_simplify(multibody(fourbar2)) prob = ODEProblem(ssys, [], (0.0, 1.4399)) # The end time is chosen to make the animation below appear to loop forever @@ -68,7 +68,7 @@ connections = [connect(j2.frame_b, b2.frame_a) @named model = ODESystem(connections, t, systems = [world; systems]) model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [], (0.0, 1.4399)) # The end time is chosen to make the animation below appear to loop forever sol2 = solve(prob, FBDF(autodiff=true)) # 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_orientation_getters.jl b/test/test_orientation_getters.jl index 3e742a53..8a69deae 100644 --- a/test/test_orientation_getters.jl +++ b/test/test_orientation_getters.jl @@ -2,7 +2,7 @@ using Test import ModelingToolkitStandardLibrary.Mechanical.Rotational @mtkmodel FurutaPendulum begin @components begin - world = W() + world = World() shoulder_joint = Revolute(n = [0, 1, 0], isroot = true, axisflange = true) elbow_joint = Revolute(n = [0, 0, 1], isroot = true, axisflange = true, phi0=0.1) upper_arm = BodyShape(; m = 1, isroot = false, r = [0, 0, 0.6], radius=0.04) @@ -30,7 +30,7 @@ end @named model = FurutaPendulum() model = complete(model) -ssys = structural_simplify(IRSystem(model)) +ssys = structural_simplify(multibody(model)) prob = ODEProblem(ssys, [model.shoulder_joint.phi => 0.0, model.elbow_joint.phi => 0.1, model.world.g => 9.81], (0, 12)) sol = solve(prob, Rodas4()) diff --git a/test/test_urdf.jl b/test/test_urdf.jl index ce467896..71d920fd 100644 --- a/test/test_urdf.jl +++ b/test/test_urdf.jl @@ -11,7 +11,7 @@ include("doublependulum.jl") using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq#, Plots import ModelingToolkit: t_nounits as t, D_nounits as D -W(args...; kwargs...) = Multibody.world + @named model = DoublePendulum() model = complete(model) ssys = structural_simplify(IRSystem(model)) diff --git a/test/test_wheels.jl b/test/test_wheels.jl index 4ecb54da..3e60bc1d 100644 --- a/test/test_wheels.jl +++ b/test/test_wheels.jl @@ -8,7 +8,7 @@ using LinearAlgebra @mtkmodel WheelInWorld begin @components begin # world = World(n=[0,0,-1]) - world = W() + world = World() wheel = RollingWheel(radius = 0.3, m = 2, I_axis = 0.06, I_long = 0.12, x0 = 0.2, @@ -59,7 +59,7 @@ using LinearAlgebra end @components begin # world = World(n=[0,0,-1]) - world = W() + world = World() wheel = RollingWheel(; radius = 0.3, m = 2, I_axis = 0.06, I_long = 0.12, x0 = 0.2, @@ -123,7 +123,7 @@ dd = diff(sol(tv, idxs=worldwheel.wheel.wheeljoint.der_angles[2]).u) # angular a import ModelingToolkitStandardLibrary.Blocks @mtkmodel WheelWithAxis begin @components begin - world = W() + world = World() prismatic = Prismatic(n = [0,1,0]) world_axis = Revolute(n = [0,1,0], iscut=false, state_priority=100, w0=10) # world_axis = RevolutePlanarLoopConstraint(n = [0,1,0]) @@ -168,7 +168,7 @@ end wheels = RollingWheelSet(radius=0.1, m_wheel=0.5, I_axis=0.01, I_long=0.02, track=0.5, state_priority=100) bar = FixedTranslation(r = [0.2, 0, 0]) body = Body(m=0.01, state_priority=1) - world = W() + world = World() end @equations begin connect(sine1.output, torque1.tau) diff --git a/test/test_worldforces.jl b/test/test_worldforces.jl index 454eccd4..d1178fb9 100644 --- a/test/test_worldforces.jl +++ b/test/test_worldforces.jl @@ -5,7 +5,7 @@ using JuliaSimCompiler using OrdinaryDiffEq doplot() = false world = Multibody.world -W(args...; kwargs...) = Multibody.world + t = Multibody.t D = Differential(t) @@ -14,7 +14,7 @@ D = Differential(t) # ============================================================================== @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) b = Body(m=1, state=true, isroot=true, quat=false, neg_w=false) @@ -46,7 +46,7 @@ sol = solve(prob, Tsit5()) # Tests here pass for any combination of quat and neg_w @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() force = WorldForce() body = Body(m=1, state=true, isroot=true, quat=false, neg_w=false) end @@ -84,7 +84,7 @@ sol = solve(prob, Tsit5()) # ============================================================================== @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:world) forceb = WorldForce(resolve_frame=:world) body = BodyShape(r=[1,0,0], state=true, isroot=true, quat=false, neg_w=true) @@ -124,7 +124,7 @@ sol = solve(prob, Tsit5(), reltol=1e-8) # ============================================================================== @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) body = BodyShape(r=[1,0,0], state=true, isroot=true, quat=false, neg_w=true) @@ -162,7 +162,7 @@ sol = solve(prob, Tsit5(), reltol=1e-8) # ============================================================================== @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) body = BodyCylinder(r=[1,0,0], state=true, isroot=true, quat=false, neg_w=true, density=1, diameter=0.1) @@ -202,7 +202,7 @@ sol = solve(prob, Tsit5(), reltol=1e-8) using LinearAlgebra @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b, radius=0.15, scale=0.2) forceb = WorldForce(resolve_frame=:frame_b, radius=0.15, scale=0.2) b0 = Body(m=1e-32, I_11=1e-32, I_22=1e-32, I_33=1e-32, state_priority=0, radius=0.14, color=[1,0,0,0.5]) @@ -244,7 +244,7 @@ sol = solve(prob, Rodas4(), reltol=1e-8) # ============================================================================== @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b, radius=0.15, scale=0.2) forceb = WorldForce(resolve_frame=:frame_b, radius=0.15, scale=0.2) b0 = Body(m=1e-32, I_11=1e-32, I_22=1e-32, I_33=1e-32, state_priority=0, radius=0.14, color=[1,0,0,0.5]) @@ -289,7 +289,7 @@ sol = solve(prob, Tsit5(), abstol=1e-8, reltol=1e-8) # ============================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================= @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) b0 = Body(m=1, state_priority=0, radius=0.1, color=[0,0,1,0.2]) @@ -325,7 +325,7 @@ reshape(sol(1, idxs = [testwf.forceb.frame_b.f; testwf.forcea.frame_b.f; testwf. @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) b0 = Body(m=1, state_priority=0, radius=0.1, color=[0,0,1,0.2]) @@ -358,7 +358,7 @@ sol = solve(prob, Tsit5()) @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) body = Body(m=1, state=true, isroot=true, quat=false, radius=0.05, color=[1,0,0,1]) @@ -392,7 +392,7 @@ sol = solve(prob, Tsit5()) # ============================================================================== @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) b0 = Body(m=1, state_priority=0) @@ -444,7 +444,7 @@ sol(1, idxs=testwf.b0.w_a) # This works with both quat and Euler @mtkmodel TestWorldForce begin @components begin - world = W() + world = World() freemotion = FreeMotion(state=true, isroot=true, quat=false) forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b)