diff --git a/Project.toml b/Project.toml index aaaa97f6..4e601ff5 100644 --- a/Project.toml +++ b/Project.toml @@ -34,7 +34,7 @@ DataInterpolations = "5, 6" FileIO = "1" Graphs = "1.0" JuliaFormatter = "1.0" -JuliaSimCompiler = "0.1.12" +JuliaSimCompiler = "0.1.26" LightXML = "0.9" LinearAlgebra = "1.6" MeshIO = "0.4" diff --git a/src/components.jl b/src/components.jl index bda1199f..82bf8c10 100644 --- a/src/components.jl +++ b/src/components.jl @@ -374,7 +374,6 @@ This component has a single frame, `frame_a`. To represent bodies with more than isroot = false, state = false, sequence = [1,2,3], - neg_w = true, phi0 = zeros(3), phid0 = zeros(3), r_0 = 0, @@ -454,7 +453,7 @@ This component has a single frame, `frame_a`. To represent bodies with more than if quat @named frame_a = Frame(varw=false) Ra = ori(frame_a, false) - qeeqs = nonunit_quaternion_equations(Ra, w_a; neg_w) + qeeqs = nonunit_quaternion_equations(Ra, w_a) else @named frame_a = Frame(varw=true) Ra = ori(frame_a, true) @@ -467,13 +466,8 @@ This component has a single frame, `frame_a`. To represent bodies with more than phid .~ D.(phi) phidd .~ D.(phid) Ra.w .~ ar.w - if neg_w - # w_a .~ -ar.w # This is required for FreeBody and ThreeSprings tests to pass, but the other one required for harmonic osciallator without joint to pass. FreeBody passes with quat=true so we use that instead - collect(w_a .~ -angular_velocity2(ar)) - else - collect(w_a .~ (angular_velocity2(ar))) + collect(w_a .~ (angular_velocity2(ar))) # w_a .~ ar.w # This one for most systems - end Ra ~ ar ] end @@ -793,7 +787,6 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of state = false quat = false sequence = [1,2,3] - neg_w = true end @parameters begin @@ -856,7 +849,7 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of frame_a = Frame() frame_b = Frame() translation = FixedTranslation(r = r) - body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sequence, neg_w, sparse_I=true) + body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sequence, sparse_I=true) end @equations begin diff --git a/src/joints.jl b/src/joints.jl index a8b578b8..35c51a7c 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -181,7 +181,6 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin z_rel_a_fixed = false, sequence = [1, 2, 3], phi = 0, phid = 0, d = 0, - neg_w = true, phidd = 0, color = [1, 1, 0, 1], radius = 0.1, @@ -224,7 +223,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin if state if quat - append!(eqs, nonunit_quaternion_equations(Rrel, w_rel; neg_w)) + append!(eqs, nonunit_quaternion_equations(Rrel, w_rel)) # append!(eqs, collect(w_rel) .~ angularVelocity2(Rrel)) else @variables begin @@ -485,7 +484,6 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi state_priority = 4, phid = 0, phidd = 0, - neg_w = true, w_rel_b = 0, r_rel_a = 0, v_rel_a = 0, @@ -546,14 +544,14 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi end if quat - append!(eqs, nonunit_quaternion_equations(Rrel, w_rel_b; neg_w)) + append!(eqs, nonunit_quaternion_equations(Rrel, w_rel_b)) else append!(eqs, [phid .~ D.(phi) phidd .~ D.(phid) Rrel ~ axes_rotations(sequence, phi, phid) - w_rel_b .~ (neg_w ? -1 : 1) * angular_velocity2(Rrel)]) + w_rel_b .~ angular_velocity2(Rrel)]) end else diff --git a/src/orientation.jl b/src/orientation.jl index 19ef65e4..e6d97e1e 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -262,7 +262,7 @@ function from_Q(Q2, w; normalize=false) Q2 = Q2 / _norm(Q2) end q = Rotations.QuatRotation(Q2, false) - R = RotMatrix(q) + R = RotMatrix(q)' RotationMatrix(R, w) end @@ -313,11 +313,11 @@ Returns a `RotationMatrix` object. """ function axis_rotation(sequence, angle; name = :R) if sequence == 1 - return RotationMatrix(Rotations.RotX(angle), zeros(3)) + return RotationMatrix(Rotations.RotX(angle)', zeros(3)) elseif sequence == 2 - return RotationMatrix(Rotations.RotY(angle), zeros(3)) + return RotationMatrix(Rotations.RotY(angle)', zeros(3)) elseif sequence == 3 - return RotationMatrix(Rotations.RotZ(angle), zeros(3)) + return RotationMatrix(Rotations.RotZ(angle)', zeros(3)) else error("Invalid sequence $sequence") end @@ -419,7 +419,7 @@ function get_frame(sol, frame, t) [R tr; 0 0 0 1] end -function nonunit_quaternion_equations(R, w; neg_w = true) +function nonunit_quaternion_equations(R, w) @variables Q(t)[1:4]=[1,0,0,0], [state_priority=-1, description="Unit quaternion with [w,i,j,k]"] # normalized @variables Q̂(t)[1:4]=[1,0,0,0], [state_priority=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized @variables Q̂d(t)[1:4]=[0,0,0,0], [state_priority=1000] @@ -435,9 +435,6 @@ function nonunit_quaternion_equations(R, w; neg_w = true) # where angularVelocity2(Q, der(Q)) = 2*([Q[4] Q[3] -Q[2] -Q[1]; -Q[3] Q[4] Q[1] -Q[2]; Q[2] -Q[1] Q[4] -Q[3]]*der_Q) # They also have w_a = angularVelocity2(frame_a.R) even for quaternions, so w_a = angularVelocity2(Q, der(Q)), this is their link between w_a and D(Q), while ours is D(Q̂) .~ (Ω * Q̂) Ω = [0 -w[1] -w[2] -w[3]; w[1] 0 w[3] -w[2]; w[2] -w[3] 0 w[1]; w[3] w[2] -w[1] 0] - if neg_w - Ω = Ω' - end # QR = from_Q(Q, angular_velocity2(Q, D.(Q))) QR = from_Q(Q̂ ./ sqrt(n), w) diff --git a/src/wheels.jl b/src/wheels.jl index 5e9e4d98..7f391ab5 100644 --- a/src/wheels.jl +++ b/src/wheels.jl @@ -117,7 +117,7 @@ this frame. @named frame_a = Frame(varw=true) Ra = ori(frame_a, true) - Rarot = axes_rotations(sequence, angles, -der_angles) # The - is the neg_w change + Rarot = axes_rotations(sequence, angles, der_angles) equations = if surface === nothing [ # Road description @@ -400,7 +400,7 @@ plot!( @named frame_a = Frame(varw=state) Ra = ori(frame_a, state) - Rarot = axes_rotations(sequence, angles, -der_angles) # The - is the neg_w change + Rarot = axes_rotations(sequence, angles, der_angles) equations = if surface === nothing [ # Road description diff --git a/test/runtests.jl b/test/runtests.jl index 0124a47c..fd0e6882 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -67,7 +67,7 @@ t = Multibody.t D = Differential(t) @testset "spring - harmonic oscillator" begin - @named body = Body(; m = 1, isroot = true, r_cm = [0, -1, 0], quat=true, neg_w=true) # This time the body isroot since there is no joint containing state + @named body = Body(; m = 1, isroot = true, r_cm = [0, -1, 0], quat=true) # This time the body isroot since there is no joint containing state @named spring = Multibody.Spring(c = 1) connections = [connect(world.frame_b, spring.frame_a) @@ -960,7 +960,16 @@ 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); +@time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=false); 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, + -0.5, -0.5, -0.5, + -0.5, -0.5, -0.5, + -0.5, -0.5, -0.5, + -0.5, -0.5, -0.5, + 0.5, 0.5, 0.5, + -0.5, -0.5, -0.5]); @test SciMLBase.successful_retcode(sol) if false import GLMakie @@ -979,7 +988,7 @@ end systems = @named begin joint = Spherical(state=true, isroot=true, phi = [π/2, 0, 0], d = 0.3) - bar = FixedTranslation(r = [0, -1, 0]) + bar = FixedTranslation(r = [0, 1, 0]) body = Body(; m = 1, isroot = false) @@ -1153,7 +1162,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, neg_w=true) +@named body = Body(; m = 1, isroot=true, quat=true) connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, rod.frame_a) @@ -1172,7 +1181,7 @@ sol1 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8) @test SciMLBase.successful_retcode(sol1) ## quat in joint -@named joint = Multibody.Spherical(isroot=true, state=true, quat=true, neg_w=true) +@named joint = Multibody.Spherical(isroot=true, state=true, quat=true) @named rod = FixedTranslation(; r = [1, 0, 0]) @named body = Body(; m = 1, isroot=false, quat=false) @@ -1193,7 +1202,7 @@ sol2 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8) @test SciMLBase.successful_retcode(sol2) ## euler -@named joint = Multibody.Spherical(isroot=true, state=true, quat=false, neg_w=true) +@named joint = Multibody.Spherical(isroot=true, state=true, quat=false) @named rod = FixedTranslation(; r = [1, 0, 0]) @named body = Body(; m = 1, isroot=false, quat=false) diff --git a/test/test_quaternions.jl b/test/test_quaternions.jl index 360db91f..92cb4094 100644 --- a/test/test_quaternions.jl +++ b/test/test_quaternions.jl @@ -75,7 +75,7 @@ using OrdinaryDiffEq, Test t = Multibody.t @named joint = Multibody.FreeMotion(isroot = true, state=false) -@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, quat=true, w_a=[1,0.5,0.2], neg_w=true) +@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, quat=true, w_a=[1,0.5,0.2]) # @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true) # @named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=false, w_a=[1,1,1]) @@ -149,7 +149,7 @@ end t = Multibody.t world = Multibody.world - @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true, neg_w=true) + @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true) @named body = Body(; m = 1, r_cm = [0.0, 0, 0]) connections = [connect(world.frame_b, joint.frame_a) @@ -203,7 +203,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, neg_w=true) + @named body = Body(; m = 1, isroot=true, quat=true, 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) @@ -272,7 +272,7 @@ using Multibody.Rotations: params @named begin body = BodyShape(m = 1, I_11 = 1, I_22 = 1, I_33 = 1, r = [0.4, 0, 0], - r_0 = [0.2, -0.5, 0.1], r_cm = [0.2, 0, 0], isroot = true, quat=true, neg_w=true) + r_0 = [0.2, -0.5, 0.1], r_cm = [0.2, 0, 0], isroot = true, quat=true) bar2 = FixedTranslation(r = [0.8, 0, 0]) spring1 = Multibody.Spring(c = 20, s_unstretched = 0) spring2 = Multibody.Spring(c = 20, s_unstretched = 0) diff --git a/test/test_wheels.jl b/test/test_wheels.jl index 3e60bc1d..75e8888d 100644 --- a/test/test_wheels.jl +++ b/test/test_wheels.jl @@ -13,7 +13,7 @@ using LinearAlgebra I_long = 0.12, x0 = 0.2, z0 = 0.2, - der_angles = [0, 5, 1]) + der_angles = [0, -5, -1]) end end @@ -78,7 +78,7 @@ defs = Dict([ worldwheel.wheel.body.r_0[1] => 0.2; worldwheel.wheel.body.r_0[2] => 0.3; worldwheel.wheel.body.r_0[3] => 0.2; - collect(worldwheel.wheel.wheeljoint.der_angles) .=> [0, 5, 1]; + collect(worldwheel.wheel.wheeljoint.der_angles) .=> [0, -5, -1]; # collect(D.(cwheel.wheel.angles)) .=> [0, 5, 1] ]) @@ -135,7 +135,7 @@ import ModelingToolkitStandardLibrary.Blocks x0 = 1, z0 = 1, iscut=true, - der_angles = [0, 5, 0]) + der_angles = [0, -5, 0]) end @equations begin connect(world.frame_b, prismatic.frame_a) diff --git a/test/test_worldforces.jl b/test/test_worldforces.jl index d1178fb9..e8788bcd 100644 --- a/test/test_worldforces.jl +++ b/test/test_worldforces.jl @@ -17,7 +17,7 @@ D = Differential(t) 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) + b = Body(m=1, state=true, isroot=true, quat=false) end @parameters begin f[1:3] @@ -48,7 +48,7 @@ sol = solve(prob, Tsit5()) @components begin world = World() force = WorldForce() - body = Body(m=1, state=true, isroot=true, quat=false, neg_w=false) + body = Body(m=1, state=true, isroot=true, quat=false) end @parameters begin f[1:3] @@ -87,7 +87,7 @@ sol = solve(prob, Tsit5()) 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) + body = BodyShape(r=[1,0,0], state=true, isroot=true, quat=false) end @parameters begin f[1:3] @@ -117,7 +117,6 @@ prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [0,1,0]], (0 sol = solve(prob, Tsit5(), reltol=1e-8) # plot(sol) @test sol(1, idxs=testwf.body.frame_a.r_0) ≈ [0.572800369240885, 0.4946715021692289, 0.0] atol=1e-3 # Spinning around center of mass -# passes with neg_w = true # ============================================================================== ## Same as above but with forces resolved in frame_b instead of world frame @@ -127,7 +126,7 @@ sol = solve(prob, Tsit5(), reltol=1e-8) 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) + body = BodyShape(r=[1,0,0], state=true, isroot=true, quat=false) end @parameters begin f[1:3] @@ -156,7 +155,7 @@ prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [0,1,0]], (0 sol = solve(prob, Tsit5(), reltol=1e-8) # plot(sol) @test sol(1, idxs=testwf.body.frame_a.r_0) ≈ [0.9419246090353689, -0.23388592078659548, 0.0] atol=1e-3 -# passes with neg_w = true + # ============================================================================== ## Same as above but with BodyCylinder instead # ============================================================================== @@ -165,7 +164,7 @@ sol = solve(prob, Tsit5(), reltol=1e-8) 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) + body = BodyCylinder(r=[1,0,0], state=true, isroot=true, quat=false, density=1, diameter=0.1) end @parameters begin f[1:3] @@ -194,7 +193,6 @@ prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [0,1,0]], (0 sol = solve(prob, Tsit5(), reltol=1e-8) # plot(sol) @test sol(0.1, idxs=testwf.body.frame_a.r_0) ≈ [0.36637355877861, 0.4818152481205165, 0.0] atol=1e-3 -# passes with neg_w = true # ============================================================================== ## First create a body and then attach Cylinder to this. The body has (almost) zero mass, so the result should be identical to the test above @@ -206,7 +204,7 @@ using LinearAlgebra 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]) - body = BodyCylinder(r=[1,0,0], density=1, diameter=0.1, state=true, isroot=true, quat=false, neg_w=true, color=[0,0,1,0.5]) + body = BodyCylinder(r=[1,0,0], density=1, diameter=0.1, state=true, isroot=true, quat=false, color=[0,0,1,0.5]) end @parameters begin f[1:3] @@ -248,7 +246,7 @@ sol = solve(prob, Rodas4(), reltol=1e-8) 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]) - body = BodyCylinder(r=[1,0,0], density=1, diameter=0.1, color=[0,0,1,0.5], state=true, isroot=true, quat=false, neg_w=true) + body = BodyCylinder(r=[1,0,0], density=1, diameter=0.1, color=[0,0,1,0.5], state=true, isroot=true, quat=false) end @parameters begin f[1:3] @@ -284,7 +282,6 @@ sol = solve(prob, Tsit5(), abstol=1e-8, reltol=1e-8) ## Simplification of the model above into two bodies # This might be the simplest test that demonstrates the problem with one force set to 0 # ============================================================================== -# We first test that it works with Euler angles and neg_w = true # ============================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================= @mtkmodel TestWorldForce begin @@ -293,7 +290,7 @@ sol = solve(prob, Tsit5(), abstol=1e-8, reltol=1e-8) 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]) - b1 = Body(m=1, state=true, isroot=true, quat=false, neg_w=true, radius=0.05, color=[1,0,0,1]) + b1 = Body(m=1, state=true, isroot=true, quat=false, radius=0.05, color=[1,0,0,1]) end @parameters begin f[1:3] @@ -315,7 +312,6 @@ prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [1,0,0]], (0 sol = solve(prob, Tsit5()) # plot(sol) @test sol(1, idxs=testwf.b1.r_0) ≈ [0, 0.0, 0.0] atol=1e-3 -# The test passes with quat=false and neg_w = true reshape(sol(1, idxs = [testwf.forceb.frame_b.f; testwf.forcea.frame_b.f; testwf.b1.frame_a.f; testwf.b0.frame_a.f;]), 3, :) @@ -351,10 +347,8 @@ ssys = structural_simplify(IRSystem(testwf)) prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [1,0,0]], (0, 1)) sol = solve(prob, Tsit5()) @test sol(1, idxs=testwf.body.r_0) ≈ [0, 0.0, 0.0] atol=1e-3 -# The test passes with quat=false and neg_w = false, but if bo is removed, it instead passes with quat=true or neg_w=true - -## Removing the body b0 from the model, the test passes with both quat=true and neg_w = false +## Removing the body b0 from the model @mtkmodel TestWorldForce begin @components begin @@ -384,8 +378,6 @@ prob = ODEProblem(ssys, [testwf.world.g => 0; collect(testwf.f) .=> [1,0,0]], (0 sol = solve(prob, Tsit5()) # plot(sol) @test sol(1, idxs=testwf.body.r_0) ≈ [0, 0.0, 0.0] atol=1e-3 -# The test passes with quat=false and neg_w = false, but if bo is removed, it instead passes with quat=true or neg_w=true - # ============================================================================== ## Almost same as above but with FixedTranslation instead of r_cm and force application at the end of the rod @@ -396,7 +388,7 @@ sol = solve(prob, Tsit5()) forcea = WorldForce(resolve_frame=:frame_b) forceb = WorldForce(resolve_frame=:frame_b) b0 = Body(m=1, state_priority=0) - body = Body(m=1, state=true, isroot=true, quat=false, neg_w=true) + body = Body(m=1, state=true, isroot=true, quat=false) tr = FixedTranslation(r=[1,0,0]) end @parameters begin @@ -427,7 +419,6 @@ sol = solve(prob, Tsit5()) # plot(sol) @test !iszero(sol(1, idxs=testwf.body.frame_a.r_0)) @test sol(1, idxs=testwf.body.frame_a.r_0) ≈ [-0.9300324062366484, 0.25508128572301375, 0.0] atol=1e-3 -# Passes with neg_w = true sol(0, idxs=testwf.forcea.frame_b.f + testwf.b0.frame_a.f + testwf.tr.frame_a.f) sol(0, idxs=testwf.forceb.frame_b.f + testwf.body.frame_a.f + testwf.tr.frame_b.f)