Skip to content

Commit

Permalink
Fix depwarn (#171)
Browse files Browse the repository at this point in the history
* Fix depwarn

* tol changes

---------

Co-authored-by: Fredrik Bagge Carlson <baggepinnen@gmail.com>
  • Loading branch information
YingboMa and baggepinnen authored Oct 7, 2024
1 parent 714fc3d commit 083796e
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 16 deletions.
17 changes: 10 additions & 7 deletions ext/Render.jl
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ import Multibody.PlanarMechanics as P
using Rotations
using LinearAlgebra
using ModelingToolkit
using ModelingToolkit: get_metadata
export render, loop_render
using MeshIO, FileIO
using StaticArrays
Expand Down Expand Up @@ -301,10 +302,11 @@ function render(model, sol,
if traces !== nothing
tvec = range(sol.t[1], stop=sol.t[end], length=500)
for frame in traces
(frame.metadata !== nothing) || error("Only frames can be traced in animations.")
if get(frame.metadata, :frame, false)
md = get_metadata(frame)
(md !== nothing) || error("Only frames can be traced in animations.")
if get(md, :frame, false)
points = get_trans(sol, frame, tvec) |> Matrix
elseif get(frame.metadata, :frame_2d, false)
elseif get(md, :frame_2d, false)
points = get_trans_2d(sol, frame, tvec) |> Matrix
else
error("Got fishy frame metadata")
Expand Down Expand Up @@ -374,10 +376,11 @@ function render(model, sol, time::Real;
if traces !== nothing
tvec = range(sol.t[1], stop=sol.t[end], length=500)
for frame in traces
(frame.metadata !== nothing) || error("Only frames can be traced in animations.")
if get(frame.metadata, :frame, false)
md = get_metadata(frame)
(md !== nothing) || error("Only frames can be traced in animations.")
if get(md, :frame, false)
points = get_trans(sol, frame, tvec) |> Matrix
elseif get(frame.metadata, :frame_2d, false)
elseif get(md, :frame_2d, false)
points = get_trans_2d(sol, frame, tvec) |> Matrix
else
error("Got fishy frame metadata")
Expand Down Expand Up @@ -1165,4 +1168,4 @@ function render!(scene, ::typeof(Multibody.CylinderVisualizer), sys, sol, t)
true
end

end
end
11 changes: 7 additions & 4 deletions src/components.jl
Original file line number Diff line number Diff line change
@@ -1,9 +1,11 @@
using LinearAlgebra
using ModelingToolkit: get_metadata
import ModelingToolkitStandardLibrary

function isroot(sys)
sys.metadata isa Dict || return false
get(sys.metadata, :isroot, false)
md = get_metadata(sys)
md isa Dict || return false
get(md, :isroot, false)
end

purple = [0.5019608f0,0.0f0,0.5019608f0,1.0f0]
Expand All @@ -13,12 +15,13 @@ purple = [0.5019608f0,0.0f0,0.5019608f0,1.0f0]
Get the orientation of `sys` as a `RotationMatrix` object. See also [`get_rot`](@ref). `ori(frame).R` is the rotation matrix that rotates a vector from the world coordinate system to the local frame.
For frames, the orientation is stored in the metadata field of the system as `sys.metadata[:orientation]`.
For frames, the orientation is stored in the metadata field of the system as `get_metadata(sys)[:orientation]`.
If `varw = true`, the angular velocity variables `w` of the frame is also included in the `RotationMatrix` object, otherwise `w` is derived as the time derivative of `R`. `varw = true` is primarily used when selecting a component as root.
"""
function ori(sys, varw = false)
if sys.metadata isa Dict && (O = get(sys.metadata, :orientation, nothing)) !== nothing
md = get_metadata(sys)
if md isa Dict && (O = get(md, :orientation, nothing)) !== nothing
R = collect(O.R)
# Since we are using this function instead of sys.ori, we need to handle namespacing properly as well
ns = nameof(sys)
Expand Down
10 changes: 5 additions & 5 deletions test/test_quaternions.jl
Original file line number Diff line number Diff line change
Expand Up @@ -118,22 +118,22 @@ for (i, ti) in enumerate(ts)
end

# Test that rotational velocity of 1 results in one full rotation in 2π seconds. Test rotation around all three major axes
@test get_R(sol, body.frame_a, 0pi) I
@test get_R(sol, body.frame_a, 0pi) I atol=1e-3
@test get_R(sol, body.frame_a, pi/2) [1 0 0; 0 0 -1; 0 1 0]' atol=1e-3
@test get_rot(sol, body.frame_a, pi/2)*[0,1,0] [0,0,1] atol=1e-3 # Same test as above, but in one other way for sanity. They y-axis basis vector is rotated around x axis which aligns it with the z-axis basis vector. get_rot is used for this, which is the transpose of get_R
@test get_R(sol, body.frame_a, 1pi) diagm([1, -1, -1]) atol=1e-3
@test get_R(sol, body.frame_a, 2pi) I atol=1e-3

prob = ODEProblem(ssys, [collect(body.w_a) .=> [0,1,0];], (0, 2pi))
sol = solve(prob, Rodas4())#,
@test get_R(sol, body.frame_a, 0pi) I
@test get_R(sol, body.frame_a, 0pi) I atol=1e-3
@test get_R(sol, body.frame_a, 1pi) diagm([-1, 1, -1]) atol=1e-3
@test get_R(sol, body.frame_a, 2pi) I atol=1e-3


prob = ODEProblem(ssys, [collect(body.w_a) .=> [0,0,1];], (0, 2pi))
sol = solve(prob, Rodas4())#,
@test get_R(sol, body.frame_a, 0pi) I
@test get_R(sol, body.frame_a, 0pi) I atol=1e-3
@test get_R(sol, body.frame_a, 1pi) diagm([-1, -1, 1]) atol=1e-3
@test get_R(sol, body.frame_a, 2pi) I atol=1e-3

Expand Down Expand Up @@ -184,7 +184,7 @@ end
@test Q Qh ./ sqrt.(n) atol=1e-2
@test norm(mapslices(norm, Q, dims=1) .- 1) < 1e-2

@test get_R(sol, joint.frame_b, 0pi) I
@test get_R(sol, joint.frame_b, 0pi) I atol=1e-3
@test_broken get_R(sol, joint.frame_b, pi/2)*[0,1,0] [0,0,1] atol=1e-3
@test get_R(sol, joint.frame_b, 1pi) diagm([1, -1, -1]) atol=1e-3
@test get_R(sol, joint.frame_b, 2pi) I atol=1e-3
Expand Down Expand Up @@ -245,7 +245,7 @@ end

Matrix(sol(ts, idxs = [body.w_a...]))

@test get_R(sol, joint.frame_b, 0pi) I
@test get_R(sol, joint.frame_b, 0pi) I atol = 1e-3


# Matrix(sol(ts, idxs = [joint.w_rel_b...]))
Expand Down

0 comments on commit 083796e

Please sign in to comment.