Skip to content

Commit

Permalink
clean up tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@baggepinnen
baggepinnen committed Oct 26, 2023
1 parent 3b28963 commit 243410b
Showing 1 changed file with 30 additions and 171 deletions.
201 changes: 30 additions & 171 deletions test/runtests.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,8 @@ using Test
using JuliaSimCompiler
using OrdinaryDiffEq
t = Multibody.t

D = Differential(t)
doplot() = false
world = Multibody.world

## Only body and world
Expand Down Expand Up @@ -37,19 +38,6 @@ The multibody paper mentions this as an interesting example, figure 8:
a spring to the environment."
=#

# With 0 mass, the LineForce should have

# !isroot
# = 2*(3+3+9+3) // 2*(r_0+f+R.T+t)
# - 2*(3+3) // 2*(f+t)
# - 2*(9-3) // 2*(R.T – R.residuals)
# = 12 equations

# isroot for one of the frames
# = 2*(3+3+9+3) // 2*(r_0+f+R.T+t)
# - 2*(3+3) // 2*(f+t)
# - 1*(9-3) // 1*(R.T – R.residuals)
# = 18 equations

@named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], phi0 = [0, 1, 0]) # This time the body isroot since there is no joint containing state
@named spring = Multibody.Spring(c = 1, fixedRotationAtFrame_a = false,
Expand All @@ -65,20 +53,20 @@ connections = [connect(world.frame_b, spring.frame_a)
irsys = IRSystem(model)
ssys = structural_simplify(irsys)
D = Differential(t)
prob = ODEProblem(ssys, [], (0, 10))

# du = prob.f.f.f_oop(prob.u0, prob.p, 0)
# @test all(isfinite, du)

@test_skip begin # Yingbo: instability
prob = ODEProblem(ssys, [], (0, 10))
sol = solve(prob, Rodas5P())
@test SciMLBase.successful_retcode(sol)
@test sol(2pi, idxs = body.r_0[1])≈0 atol=1e-3
@test sol(2pi, idxs = body.r_0[2])≈1 atol=1e-3
@test sol(2pi, idxs = body.r_0[3])≈0 atol=1e-3
@test sol(pi, idxs = body.r_0[2]) < -2

isinteractive() &&
doplot() &&
plot(sol, idxs = [collect(body.r_0); collect(body.v_0); collect(body.phi)], layout = 9)
end

Expand Down Expand Up @@ -123,7 +111,7 @@ sol = solve(prob, Rodas4())
@test maximum(norm.(eachcol(reduce(hcat, sol[collect(forcesensor.force.u)])))) ≈
maximum(norm.(eachcol(reduce(hcat, sol[collect(joint.frame_a.f)]))))

isinteractive() && plot(sol, idxs = collect(joint.phi))
doplot() && plot(sol, idxs = collect(joint.phi))

# ==============================================================================
## Simple pendulum from Modelica "First Example" tutorial ======================
Expand Down Expand Up @@ -157,7 +145,7 @@ sol = solve(prob, Rodas4())
@test SciMLBase.successful_retcode(sol)
@test minimum(sol[rev.phi]) > -π
@test sol[rev.phi][end]≈-π / 2 rtol=0.01 # pendulum settles at 90 degrees stable equilibrium
isinteractive() && plot(sol, idxs = collect(rev.phi))
doplot() && plot(sol, idxs = collect(rev.phi))

# ==============================================================================
## Simple pendulum with rod ====================================================
Expand Down Expand Up @@ -188,7 +176,7 @@ sol2 = solve(prob, Rodas4())
@test SciMLBase.successful_retcode(sol2)
@test minimum(sol2[rev.phi]) > -π
@test sol2[rev.phi][end]≈-π / 2 rtol=0.01 # pendulum settles at 90 degrees stable equilibrium
isinteractive() && plot(sol2, idxs = collect(rev.phi))
doplot() && plot(sol2, idxs = collect(rev.phi))

@test sol2(1:10, idxs=rev.phi).u ≈ sol(1:10, idxs=rev.phi).u atol=1e-2

Expand Down Expand Up @@ -222,7 +210,7 @@ sol3 = solve(prob, Rodas4())
@test SciMLBase.successful_retcode(sol2)
@test minimum(sol3[rev.phi]) > -π
@test sol3[rev.phi][end]≈-π / 2 rtol=0.01 # pendulum settles at 90 degrees stable equilibrium
isinteractive() && plot(sol3, idxs = rev.phi)
doplot() && plot(sol3, idxs = rev.phi)
@test sol3(1:10, idxs=rev.phi).u ≈ sol(1:10, idxs=rev.phi).u atol=1e-2

# ==============================================================================
Expand Down Expand Up @@ -280,7 +268,7 @@ sol = solve(prob, Rodas4())
@test sol[rev1.phi][end]≈-π / 2 rtol=0.01 # pendulum settles at 90 degrees stable equilibrium
@test_broken sol[rev2.phi][end]≈0 rtol=0.01 # pendulum settles at 90 degrees stable equilibrium which is 0 relative rotation compared to rev1
@test sol[body2.r_0[2]][end]≈-2 rtol=0.01 # sum of rod lengths = 2
isinteractive() &&
doplot() &&
plot(sol, idxs = [rev1.phi; rev2.phi; damper2.phi_rel; collect(body2.r_0[1:2])])

# ==============================================================================
Expand All @@ -305,7 +293,7 @@ prob = ODEProblem(ssys, [damper.s_rel => 1, D(joint.s) => 0, D(D(joint.s)) => 0]
sol = solve(prob, Rodas4())
@test SciMLBase.successful_retcode(sol)
@test sol[joint.s][end]≈-9.81 rtol=0.01 # gravitational acceleration since spring stiffness is 1
isinteractive() && plot(sol, idxs = joint.s)
doplot() && plot(sol, idxs = joint.s)

# ==============================================================================
## Spring damper system from https://www.maplesoft.com/documentation_center/online_manuals/modelica/Modelica_Mechanics_MultiBody_Examples_Elementary.html#Modelica.Mechanics.MultiBody.Examples.Elementary.SpringDamperSystem
Expand Down Expand Up @@ -371,68 +359,19 @@ sol = solve(prob, Rodas4())
@test SciMLBase.successful_retcode(sol)
@test sol(sol.t[end], idxs = spring1.v)≈0 atol=0.01 # damped oscillation

isinteractive() && plot(sol, idxs = [spring1.s, spring2.s])
isinteractive() && plot(sol, idxs = [body1.r_0[2], body2.r_0[2]])
isinteractive() && plot(sol, idxs = [spring1.f, spring2.f])
doplot() && plot(sol, idxs = [spring1.s, spring2.s])
doplot() && plot(sol, idxs = [body1.r_0[2], body2.r_0[2]])
doplot() && plot(sol, idxs = [spring1.f, spring2.f])

#=
model ThreeSprings "3-dim. springs in series and parallel connection"
extends Modelica.Icons.Example;
parameter Boolean animation=true "= true, if animation shall be enabled";
inner Modelica.Mechanics.MultiBody.World world(animateWorld=animation);
Modelica.Mechanics.MultiBody.Parts.Body body1(
animation=animation,
r_CM={0,-0.2,0},
m=0.8,
I_11=0.1,
I_22=0.1,
I_33=0.1,
sphereDiameter=0.2,
r_0(start={0.5,-0.3,0}, fixed=true),
v_0(fixed=true),
angles_fixed=true,
w_0_fixed=true);
Modelica.Mechanics.MultiBody.Parts.FixedTranslation bar1(animation=animation, r={0.3,0,0});
Modelica.Mechanics.MultiBody.Forces.Spring spring1(
lineForce(r_rel_0(start={-0.2,-0.2,0.2})),
s_unstretched=0.1,
width=0.1,
coilWidth=0.005,
numberOfWindings=5,
c=20,
animation=animation);
Modelica.Mechanics.MultiBody.Parts.FixedTranslation bar2(animation=animation, r={0,0,0.3});
Modelica.Mechanics.MultiBody.Forces.Spring spring2(
s_unstretched=0.1,
width=0.1,
coilWidth=0.005,
numberOfWindings=5,
c=40,
animation=animation);
Modelica.Mechanics.MultiBody.Forces.Spring spring3(
s_unstretched=0.1,
width=0.1,
coilWidth=0.005,
numberOfWindings=5,
c=20,
animation=animation);
equation
connect(world.frame_b, bar1.frame_a);
connect(world.frame_b, bar2.frame_a);
connect(bar1.frame_b, spring1.frame_a);
connect(bar2.frame_b, spring3.frame_a);
connect(spring2.frame_b, body1.frame_a);
connect(spring3.frame_b, spring1.frame_b);
connect(spring2.frame_a, spring1.frame_b);
end ThreeSprings;
=#
##
# ==============================================================================
## Three springs ===============================================================
# ==============================================================================
# https://doc.modelica.org/om/Modelica.Mechanics.MultiBody.Examples.Elementary.ThreeSprings.html
using Multibody
using ModelingToolkit
using JuliaSimCompiler
using OrdinaryDiffEq

# https://doc.modelica.org/om/Modelica.Mechanics.MultiBody.Examples.Elementary.ThreeSprings.html

t = Multibody.t
D = Differential(t)
Expand Down Expand Up @@ -476,53 +415,14 @@ prob = ODEProblem(ssys, [], (0, 10))
sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-1 .* rand.()) # Yingbo: init not working unless random
@test SciMLBase.successful_retcode(sol)

isinteractive() && plot(sol, idxs = [body1.r_0...])
doplot() && plot(sol, idxs = [body1.r_0...])
# end
# TODO: add tutorial explaining what interesting things this demos illustrates
# fixedRotationAtFrame_a and b = true required

## FreeBody
#=
model FreeBody
"Free flying body attached by two springs to environment"
extends Modelica.Icons.Example;
parameter Boolean animation=true "= true, if animation shall be enabled";
inner Modelica.Mechanics.MultiBody.World world;
Modelica.Mechanics.MultiBody.Parts.FixedTranslation bar2(r={0.8,0,0}, animation=false);
Modelica.Mechanics.MultiBody.Forces.Spring spring1(
width=0.1,
coilWidth=0.005,
numberOfWindings=5,
c=20,
s_unstretched=0);
Modelica.Mechanics.MultiBody.Parts.BodyShape body(
m=1,
I_11=1,
I_22=1,
I_33=1,
r={0.4,0,0},
r_CM={0.2,0,0},
width=0.05,
r_0(start={0.2,-0.5,0.1}, fixed=true),
v_0(fixed=true),
angles_fixed=true,
w_0_fixed=true,
angles_start={0.174532925199433,0.174532925199433,0.174532925199433});
Modelica.Mechanics.MultiBody.Forces.Spring spring2(
c=20,
s_unstretched=0,
width=0.1,
coilWidth=0.005,
numberOfWindings=5);
equation
connect(bar2.frame_a, world.frame_b);
connect(spring1.frame_b, body.frame_a);
connect(bar2.frame_b, spring2.frame_a);
connect(spring1.frame_a, world.frame_b);
connect(body.frame_b, spring2.frame_b);
end FreeBody;
=#

# ==============================================================================
## FreeBody ====================================================================
# ==============================================================================
# https://doc.modelica.org/om/Modelica.Mechanics.MultiBody.Examples.Elementary.FreeBody.html
using Multibody
using ModelingToolkit
Expand Down Expand Up @@ -568,7 +468,7 @@ prob = ODEProblem(ssys,
sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-2 .* rand.()) # Yingbo: init not working unless random
@assert SciMLBase.successful_retcode(sol)

isinteractive() && plot(sol, idxs = [body.r_0...])
doplot() && plot(sol, idxs = [body.r_0...])
# end

# ==============================================================================
Expand Down Expand Up @@ -612,7 +512,7 @@ prob = ODEProblem(ssys,
sol = solve(prob, Rodas4())
@assert SciMLBase.successful_retcode(sol)

isinteractive() && plot(sol, idxs = [body.r_0...])
doplot() && plot(sol, idxs = [body.r_0...])

@test norm(sol(0, idxs = [body.r_0...])) ≈ 1
@test norm(sol(10, idxs = [body.r_0...])) ≈ 1
Expand Down Expand Up @@ -647,58 +547,17 @@ prob = ODEProblem(ssys,
], (0, 10))
sol2 = solve(prob, Rodas4())
@test SciMLBase.successful_retcode(sol2)
plot(sol2, idxs = [body.r_0...])

@test norm(sol2(0, idxs = [body.r_0...])) ≈ 1
@test norm(sol2(10, idxs = [body.r_0...])) ≈ 1

doplot() && plot(sol2, idxs = [body.r_0...])

# ==============================================================================
## GearConstraint ===================================================
# ==============================================================================
# https://doc.modelica.org/om/Modelica.Mechanics.MultiBody.Examples.Rotational3DEffects.GearConstraint.html

#=
model GearConstraint
extends Modelica.Icons.Example;
Joints.GearConstraint gearConstraint( ratio=10);
inner World world( driveTrainMechanics3D=true,
g=0);
Parts.BodyCylinder cyl1(
diameter=0.1,
color={0,128,0},
r={0.4,0,0});
Parts.BodyCylinder cyl2( r={0.4,0,0}, diameter=
0.2);
Forces.Torque torque1;
Blocks.Sources.Sine sine[ 3](amplitude={2,0,0}, freqHz={1,1,1});
Parts.Fixed fixed;
Rotational.Components.Inertia inertia1(
J=cyl1.I[1, 1],
a(fixed=false),
phi(fixed=true),
w(fixed=true));
Rotational.Components.IdealGear idealGear( ratio=10, useSupport=true);
Rotational.Components.Inertia inertia2( J=cyl2.I[1, 1]);
Rotational.Sources.Torque torque2(useSupport=true);
Parts.Mounting1D mounting1D;
equation
connect(world.frame_b,gearConstraint. bearing);
connect(cyl1.frame_b,gearConstraint. frame_a);
connect(gearConstraint.frame_b,cyl2. frame_a);
connect(torque1.frame_b,cyl1. frame_a);
connect(torque1.frame_a,world. frame_b);
connect(sine.y,torque1. torque);
connect(inertia1.flange_b,idealGear. flange_a);
connect(idealGear.flange_b,inertia2. flange_a);
connect(torque2.flange,inertia1. flange_a);
connect(sine[1].y,torque2. tau);
connect(mounting1D.flange_b,idealGear.support);
connect(mounting1D.flange_b,torque2.support);
connect(fixed.frame_b,mounting1D. frame_a);
end GearConstraint;
=#
##
using Multibody
using ModelingToolkit
using JuliaSimCompiler
Expand Down Expand Up @@ -741,12 +600,12 @@ eqs = [connect(world.frame_b, gearConstraint.bearing)
@named model = ODESystem(eqs, t, systems = [world; systems])


ssys = structural_simplify(IRSystem(model))

@test_skip begin
# ssys = structural_simplify(model) # Yingbo: Sym doesn't have a operation or arguments!
ssys = structural_simplify(IRSystem(model)) # Yingbo: Not a well formed derivative expression

prob = ODEProblem(ssys,
prob = ODEProblem(ssys, # Yingbo: Not a well formed derivative expression
[
D(gearConstraint.actuatedRevolute_b.phi) => 0,
D(inertia2.flange_a.phi) => 0,
Expand Down Expand Up @@ -776,7 +635,7 @@ defs = [
ssys = structural_simplify(IRSystem(wheel))
prob = ODEProblem(ssys, defs, (0, 10))
@test_skip begin # Does not initialize
sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-2 .* rand.())
sol = solve(prob, Rodas4())#, u0 = prob.u0 .+ 1e-2 .* rand.())
@info "Write tests"
end

Expand Down Expand Up @@ -806,7 +665,7 @@ ssys = structural_simplify(IRSystem(model))
prob = ODEProblem(ssys, [], (0, 10))

sol = solve(prob, Rodas4())
isinteractive() && plot(sol, idxs = body.r_0[2], title = "Free falling body")
doplot() && plot(sol, idxs = body.r_0[2], title = "Free falling body")
y = sol(0:0.1:10, idxs = body.r_0[2])
@test y≈-9.81 / 2 .* (0:0.1:10) .^ 2 atol=1e-2 # Analytical solution to acceleration problem

Expand All @@ -830,7 +689,7 @@ ssys = structural_simplify(IRSystem(model))
prob = ODEProblem(ssys, [], (0, 10))

sol = solve(prob, Rodas4())
isinteractive() && plot(sol, idxs = body.r_0[2], title = "Free falling body")
doplot() && plot(sol, idxs = body.r_0[2], title = "Free falling body")
y = sol(0:0.1:10, idxs = body.r_0[2])
@test y≈-9.81 / 2 .* (0:0.1:10) .^ 2 atol=1e-2 # Analytical solution to acceleration problem

Expand Down Expand Up @@ -865,7 +724,7 @@ prob = ODEProblem(ssys,
(0, 100))

sol = solve(prob, Rodas4())
isinteractive() && plot(sol, idxs = collect(freeMotion.phi), title = "Dzhanibekov effect")
doplot() && plot(sol, idxs = collect(freeMotion.phi), title = "Dzhanibekov effect")
@info "Write tests"

@test_broken sol(0, idxs = collect(freeMotion.phi)) != zeros(3)

0 comments on commit 243410b

Please sign in to comment.