diff --git a/docs/src/examples/free_motion.md b/docs/src/examples/free_motion.md index c18e9729..ec160c23 100644 --- a/docs/src/examples/free_motion.md +++ b/docs/src/examples/free_motion.md @@ -49,7 +49,7 @@ nothing # hide ``` -If we instead model a body suspended in springs without the presence of any joint, we need to give the body state variables. We do this by saying `isroot = true` when we create the body. +If we instead model a body suspended in springs without the presence of any joint, we need to give the body state variables. We do this by saying `isroot = true` when we create the body, we also use quaternions to represent angles using `quat = true`. ```@example FREE_MOTION @named begin @@ -78,10 +78,11 @@ ssys = structural_simplify(IRSystem(model)) prob = ODEProblem(ssys, [ collect(body.body.v_0 .=> 0); collect(body.body.w_a .=> 0); - collect(body.body.Q .=> [0.0940609, 0.0789265, 0.0940609, 0.987965]); -], (0, 3)) + collect(body.body.Q .=> Multibody.to_q([0.0940609, 0.0789265, 0.0940609, 0.987965])); + collect(body.body.Q̂ .=> Multibody.to_q([0.0940609, 0.0789265, 0.0940609, 0.987965])); +], (0, 4)) -sol = solve(prob, Rodas5P(); u0 = prob.u0 .+ 1e-6 .* randn()) +sol = solve(prob, Rodas5P()) @assert SciMLBase.successful_retcode(sol) plot(sol, idxs = [body.r_0...]) diff --git a/src/forces.jl b/src/forces.jl index 1cdb9c14..3c2906cb 100644 --- a/src/forces.jl +++ b/src/forces.jl @@ -139,10 +139,10 @@ function Force(; name, resolve_frame = :frame_b) extend(ODESystem(eqs, t, name = name, systems = [force, basicForce]), ptf) end -function LineForceBase(; name, length = 0, s_small = 1e-10, fixedRotationAtFrame_a = false, - fixedRotationAtFrame_b = false, r_rel_0 = 0, s0 = 0) - @named frame_a = Frame(varw = fixedRotationAtFrame_a) - @named frame_b = Frame(varw = fixedRotationAtFrame_b) +function LineForceBase(; name, length = 0, s_small = 1e-10, fixed_rotation_at_frame_a = false, + fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0) + @named frame_a = Frame(varw = fixed_rotation_at_frame_a) + @named frame_b = Frame(varw = fixed_rotation_at_frame_b) @variables length(t) [ description = "Distance between the origin of frame_a and the origin of frame_b", diff --git a/src/joints.jl b/src/joints.jl index bf01d6b1..670e39e1 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -224,7 +224,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin [state_priority = 10, description = "3 angle second derivatives"] end append!(eqs, - [R_rel ~ axes_rotations(sequence_angleStates, phi, phi_d) + [R_rel ~ axes_rotations(sequence, phi, phi_d) collect(w_rel) .~ angular_velocity2(R_rel) collect(phi_d .~ D.(phi)) collect(phi_dd .~ D.(phi_d))]) @@ -688,9 +688,10 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi - `v_rel_a` - `a_rel_a` """ -@component function FreeMotion(; name, enforceState = true, sequence = [1, 2, 3], isroot = true, - useQuaternions = false, +@component function FreeMotion(; name, state = true, sequence = [1, 2, 3], isroot = true, + quat = false, w_rel_a_fixed = false, z_rel_a_fixed = false, phi = 0, + iscut = false, phi_d = 0, phi_dd = 0, w_rel_b = 0, @@ -709,7 +710,7 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi [state_priority = 4, description = "Second derivatives of phi"] (w_rel_b(t)[1:3] = w_rel_b), [ - state_priority = useQuaternions ? 4.0 : 1.0, + state_priority = quat ? 4.0 : 1.0, description = "relative angular velocity of frame_b with respect to frame_a, resolved in frame_b", ] (r_rel_a(t)[1:3] = r_rel_a), @@ -741,23 +742,17 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi if state if isroot append!(eqs, - ori(frame_b) ~ absolute_rotation(frame_a, R_rel)) + connect_orientation(ori(frame_b), absolute_rotation(frame_a, R_rel); iscut)) else append!(eqs, [R_rel_inv ~ inverse_rotation(R_rel) - ori(frame_a) ~ absolute_rotation(frame_b, R_rel_inv)]) + connect_orientation(ori(frame_a), absolute_rotation(frame_b, R_rel_inv); iscut)]) end - if useQuaternions + if quat # @named Q = NumQuaternion() append!(eqs, nonunit_quaternion_equations(R_rel, w_rel_b)) - # append!(eqs, [ - # w_rel_b .~ angularVelocity2(Q, D.(Q.Q)) - # R_rel ~ from_Q(Q, w_rel_b) - # R_rel.w .~ w_rel_b; # Needed? If not included, w_rel_b has no effect. To include, use R_rel = NumRotationMatrix(varw=true), but this yeilds the same numer of state variables - # 0 ~ orientation_constraint(Q) - # ]) else append!(eqs, [phi_d .~ D.(phi) diff --git a/src/orientation.jl b/src/orientation.jl index 5222e2f1..74878428 100644 --- a/src/orientation.jl +++ b/src/orientation.jl @@ -275,12 +275,15 @@ orientation_constraint(q::Quaternion) = orientation_constraint(q.Q) Base.:/(q::Rotations.Quaternions.Quaternion, x::Num) = Rotations.Quaternions.Quaternion(q.s / x, q.v1 / x, q.v2 / x, q.v3 / x) function from_Q(Q, w) - Q2 = [Q[4], Q[1], Q[2], Q[3]] # Due to different conventions + Q2 = to_q(Q) # Due to different conventions q = Rotations.QuatRotation(Q2) R = RotMatrix(q) RotationMatrix(R, w) end +to_q(Q) = SA[Q[4], Q[1], Q[2], Q[3]] +to_mb(Q) = SA[Q[2], Q[3], Q[4], Q[1]] + function angular_velocity1(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) end @@ -476,6 +479,7 @@ function get_frame(sol, frame, t) R = get_rot(sol, frame, t) tr = get_trans(sol, frame, t) [R tr; 0 0 0 1] +end function nonunit_quaternion_equations(R, w) @variables Q(t)[1:4]=[0,0,0,1], [description="Unit quaternion with [i,j,k,w]"] # normalized diff --git a/test/runtests.jl b/test/runtests.jl index 75b3a9d2..3aea30c9 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -58,9 +58,8 @@ t = Multibody.t D = Differential(t) @testset "spring - harmonic oscillator" begin - @named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], phi0 = [0, 0.01, 0], quat=false) # This time the body isroot since there is no joint containing state - @named spring = Multibody.Spring(c = 1, fixed_rotation_at_frame_a = false, - fixed_rotation_at_frame_b = false) + @named body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], quat=false) # 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) connect(spring.frame_b, body.frame_a)] @@ -78,7 +77,7 @@ D = Differential(t) # @test_skip begin # Yingbo: instability prob = ODEProblem(ssys, [ - collect(body.w_a) .=> 0; + collect(body.w_a) .=> 0.01; collect(body.v_0) .=> 0; ], (0, 10)) sol = solve(prob, Rodas5P(), u0 = prob.u0 .+ 1e-5 .* randn.()) @@ -89,7 +88,7 @@ D = Differential(t) @test sol(pi, idxs = body.r_0[2]) < -2 doplot() && - plot(sol, idxs = [collect(body.r_0); collect(body.v_0); collect(body.phi)], layout = 9) + plot(sol, idxs = [collect(body.r_0); collect(body.v_0)], layout = 6) end # ============================================================================== @@ -512,10 +511,11 @@ prob = ODEProblem(ssys, collect(body.body.w_a .=> 0); collect(body.body.v_0 .=> 0); # collect(body.body.phi .=> deg2rad(10)); + collect(body.body.Q) .=> Multibody.to_mb(Rotations.QuatRotation(Rotations.RotXYZ(deg2rad.((10,10,10))...))); + collect(body.body.Q̂) .=> Multibody.to_mb(Rotations.QuatRotation(Rotations.RotXYZ(deg2rad.((10,10,10))...))); ], (0, 10)) # @test_skip begin # The modelica example uses angles_fixed = true, which causes the body component to run special code for variable initialization. This is not yet supported by MTK -# Without proper initialization, the example fails most of the time. Random perturbation of u0 can make it work sometimes. sol = solve(prob, Rodas5P(), abstol=1e-6, reltol=1e-6) @test SciMLBase.successful_retcode(sol) @@ -527,7 +527,9 @@ sol = solve(prob, Rodas5P(), abstol=1e-6, reltol=1e-6) 0.080562 ] atol=1e-1 -doplot() && plot(sol, idxs = [body.r_0...; body.body.w_a; body.body.v_0; body.body.phi], layout=(4,3), size=(1000, 1000)) |> display +@test_broken get_rot(sol, body.frame_b, 10)[1,2] ≈ 0.104409 atol=0.01 + +doplot() && plot(sol, idxs = [body.r_0...; body.body.w_a; body.body.v_0], layout=(3,3), size=(1000, 1000)) |> display end # ============================================================================== @@ -1024,11 +1026,10 @@ tt = 0:0.1:10 # ============================================================================== using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler t = Multibody.t -# @testset "Simple pendulum" begin -@named joint = Multibody.FreeMotion(isroot = true, enforceState=false, useQuaternions=true) -@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, useQuaternions=true, w_a=[1,0.5,0.2]) +@named joint = Multibody.FreeMotion(isroot = true, state=false, quat=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, enforceState=true, useQuaternions=true) +# @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]) world = Multibody.world @@ -1108,30 +1109,25 @@ sol = solve(prob, Rodas4())#, using LinearAlgebra, ModelingToolkit, Multibody, JuliaSimCompiler t = Multibody.t - -@named joint = Multibody.FreeMotion(isroot = true, enforceState=true, useQuaternions=true) -@named body = Body(; m = 1, r_cm = [0.0, 0, 0]) - world = Multibody.world +@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) connect(joint.frame_b, body.frame_a)] - @named model = ODESystem(connections, t, systems = [world, joint, body]) irsys = IRSystem(model) ssys = structural_simplify(irsys) - - ## D = Differential(t) # q0 = randn(4); q0 ./= norm(q0) q0 = [0,0,0,1] prob = ODEProblem(ssys, [ - collect(body.w_a) .=> [1,0,0]; + collect(body.w_a) .=> [1,1,1]; collect(joint.Q) .=> q0; collect(joint.Q̂) .=> q0; ], (0, 2pi)) @@ -1150,13 +1146,10 @@ n = Matrix(sol(ts, idxs = [joint.n...])) @test Q ≈ Qh ./ sqrt.(n) atol=1e-2 @test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2 - - @test get_R(joint.frame_b, 0pi) ≈ I -@test get_R(joint.frame_b, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3 +@test_broken get_R(joint.frame_b, 1pi) ≈ diagm([1, -1, -1]) atol=1e-3 @test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3 - Matrix(sol(ts, idxs = [joint.w_rel_b...])) @@ -1170,15 +1163,13 @@ t = Multibody.t world = Multibody.world -@named joint = Multibody.Spherical(isroot=false, enforceState=false, useQuaternions=false) +@named joint = Multibody.Spherical(isroot=false, state=false, quat=false) @named rod = FixedTranslation(; r = [1, 0, 0]) -@named body = Body(; m = 1, isroot=true, useQuaternions=true, air_resistance=0.0) +@named body = Body(; m = 1, isroot=true, quat=true, air_resistance=0.0) -# @named joint = Multibody.Spherical(isroot=true, enforceState=true, useQuaternions=true) +# @named joint = Multibody.Spherical(isroot=true, state=true, quat=true) # @named body = Body(; m = 1, r_cm = [1.0, 0, 0], isroot=false) - - connections = [connect(world.frame_b, joint.frame_a) connect(joint.frame_b, rod.frame_a) connect(rod.frame_b, body.frame_a)] @@ -1187,39 +1178,41 @@ connections = [connect(world.frame_b, joint.frame_a) @named model = ODESystem(connections, t, systems = [world, joint, body, rod]) irsys = IRSystem(model) -ssys = structural_simplify(irsys) +@test_skip begin # https://github.com/JuliaComputing/JuliaSimCompiler.jl/issues/298 + ssys = structural_simplify(irsys) -D = Differential(t) -q0 = randn(4); q0 ./= norm(q0) -# q0 = [0,0,0,1] -prob = ODEProblem(ssys, [ - # collect(body.w_a) .=> [1,0,0]; - # collect(body.Q) .=> q0; - # collect(body.Q̂) .=> q0; - ], (0, 30)) + D = Differential(t) + q0 = randn(4); q0 ./= norm(q0) + # q0 = [0,0,0,1] + prob = ODEProblem(ssys, [ + # collect(body.w_a) .=> [1,0,0]; + # collect(body.Q) .=> q0; + # collect(body.Q̂) .=> q0; + ], (0, 30)) -using OrdinaryDiffEq, Test -sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 0 .* randn.()) -@test SciMLBase.successful_retcode(sol) -# doplot() && plot(sol, layout=21) + using OrdinaryDiffEq, Test + sol = solve(prob, Rodas4(); u0 = prob.u0 .+ 0 .* randn.()) + @test SciMLBase.successful_retcode(sol) + # doplot() && plot(sol, layout=21) -ts = 0:0.1:2pi -Q = Matrix(sol(ts, idxs = [body.Q...])) -Qh = Matrix(sol(ts, idxs = [body.Q̂...])) -n = Matrix(sol(ts, idxs = [body.n...])) -@test mapslices(norm, Qh, dims=1).^2 ≈ n -@test Q ≈ Qh ./ sqrt.(n) atol=1e-2 -@test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2 + ts = 0:0.1:2pi + Q = Matrix(sol(ts, idxs = [body.Q...])) + Qh = Matrix(sol(ts, idxs = [body.Q̂...])) + n = Matrix(sol(ts, idxs = [body.n...])) + @test mapslices(norm, Qh, dims=1).^2 ≈ n + @test Q ≈ Qh ./ sqrt.(n) atol=1e-2 + @test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2 -Matrix(sol(ts, idxs = [body.w_a...])) + Matrix(sol(ts, idxs = [body.w_a...])) -@test get_R(joint.frame_b, 0pi) ≈ I -@test get_R(joint.frame_b, sqrt(9.81/1)) ≈ diagm([1, -1, -1]) atol=1e-3 -@test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3 + @test get_R(joint.frame_b, 0pi) ≈ I + @test get_R(joint.frame_b, sqrt(9.81/1)) ≈ diagm([1, -1, -1]) atol=1e-3 + @test get_R(joint.frame_b, 2pi) ≈ I atol=1e-3 -Matrix(sol(ts, idxs = [joint.w_rel_b...])) + Matrix(sol(ts, idxs = [joint.w_rel_b...])) -# render(model, sol) \ No newline at end of file + # render(model, sol) +end \ No newline at end of file