diff --git a/Project.toml b/Project.toml index e85d36a70..df34ea35c 100644 --- a/Project.toml +++ b/Project.toml @@ -18,8 +18,8 @@ Meshing = "e6723b4c-ebff-59f1-b4b7-d97aa5274f73" Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a" Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" Polyhedra = "67491407-f73d-577b-9b50-8179a7c68029" +Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" -Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" Scratch = "6c6a2e73-6563-6170-7368-637461726353" SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" @@ -27,19 +27,19 @@ Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [compat] -Colors = "<0.12.6, 0.12.6" -DocStringExtensions = "<0.8.3, 0.8.3" +Colors = "<0.12.8, 0.12.8" +DocStringExtensions = "<0.8.6, 0.8.6" FFMPEG = "0.4.1" -GeometryBasics = "0.3" -JLD2 = "0.4.21" -LaTeXStrings = "1.3.0" -LightGraphs = "1.3.5" +GeometryBasics = "<0.4.0, 0.4.0" +JLD2 = "<0.4.22, 0.4.22" +LaTeXStrings = "1.3" +LightGraphs = "1.3" LightXML = "<0.9.0, 0.9.0" -MeshCat = "0.14" -Meshing = "0.5.7" -Parameters = "0.12" -Polyhedra = "0.6.18" -Rotations = "1.0.2 - 1.0.2" +MeshCat = "<0.14.0, 0.14.0" +Meshing = "<0.5.7, 0.5.7" +Parameters = "<0.12.0, 0.12.0" +Polyhedra = "<0.7.1, 0.7.1" +Quaternions = "<0.5.3, 0.5.3" Scratch = "1.1" -StaticArrays = "1.4" -julia = "1.6" +StaticArrays = "1.2" +julia = "1.6, 1.7" diff --git a/environments/box/methods/initialize.jl b/environments/box/methods/initialize.jl index 362afe146..a814dbe36 100644 --- a/environments/box/methods/initialize.jl +++ b/environments/box/methods/initialize.jl @@ -60,7 +60,7 @@ end function initialize_box!(mechanism::Mechanism{T}; x=[0.0, 0.0, 1.0], - q=UnitQuaternion(1.0, 0.0, 0.0, 0.0), + q=Quaternion(1.0, 0.0, 0.0, 0.0), v=[1.0, 0.3, 0.2], ω=[2.5, -1.0, 2.0]) where T diff --git a/environments/box2D/methods/initialize.jl b/environments/box2D/methods/initialize.jl index aacc136ea..e35bfa6d6 100644 --- a/environments/box2D/methods/initialize.jl +++ b/environments/box2D/methods/initialize.jl @@ -74,7 +74,7 @@ function initialize_box2D!(mechanism::Mechanism{T}; set_maximal_configurations!(body, x=[0.0; position] + [0.0, 0.0 , z], - q=UnitQuaternion(RotX(orientation))) + q=RotX(orientation)) set_maximal_velocities!(body, v=[0.0; linear_velocity], ω=[angular_velocity, 0.0, 0.0]) diff --git a/environments/cartpole/methods/env.jl b/environments/cartpole/methods/env.jl index fba35d585..4bc8ffba3 100644 --- a/environments/cartpole/methods/env.jl +++ b/environments/cartpole/methods/env.jl @@ -158,7 +158,7 @@ function ghost(env::Environment{Cartpole}, z_sol; for (i, xt) in enumerate(z_sol) z_pendulum = xt[13 .+ (1:13)] x = z_pendulum[1:3] - q = UnitQuaternion(z_pendulum[6 .+ (1:4)]...) + q = Quaternion(z_pendulum[6 .+ (1:4)]...) k = x + vector_rotate([0.0; 0.0; -0.5], q) push!(points, Point(k[1], k[2], k[3])) diff --git a/environments/cartpole/methods/initialize.jl b/environments/cartpole/methods/initialize.jl index d42f39edc..6c1e8d4b6 100644 --- a/environments/cartpole/methods/initialize.jl +++ b/environments/cartpole/methods/initialize.jl @@ -17,7 +17,7 @@ function get_cartpole(; # Links origin = Origin{Float64}() slider = Capsule(1.5 * radius, slider_length, slider_mass, - axis_offset=UnitQuaternion(RotX(0.5 * π)), + axis_offset=RotX(0.5 * π), color=RGBA(0.7, 0.7, 0.7, 1.0)) pendulum = Capsule(radius, pendulum_length, pendulum_mass, color=RGBA(0.7, 0.7, 0.7, 1.0)) @@ -56,14 +56,14 @@ function initialize_cartpole!(mech::Mechanism{T,Nn,Ne,Nb}; if mode == :down set_maximal_configurations!(mech.bodies[1], mech.bodies[2], Δx=[0.0; 0.0; -0.5 * pendulum_length], - Δq=UnitQuaternion(RotX(π))) + Δq=RotX(π)) set_maximal_velocities!(mech.bodies[2], v=zeros(3), ω=zeros(3)) elseif mode == :up set_maximal_configurations!(mech.bodies[1], mech.bodies[2], Δx=[0.0; 0.0; 0.5 * pendulum_length], - Δq=UnitQuaternion(RotX(π))) + Δq=RotX(π)) set_maximal_velocities!(mech.bodies[2], v=zeros(3), ω=zeros(3)) diff --git a/environments/dzhanibekov/methods/initialize.jl b/environments/dzhanibekov/methods/initialize.jl index ee047af63..10ba1642a 100644 --- a/environments/dzhanibekov/methods/initialize.jl +++ b/environments/dzhanibekov/methods/initialize.jl @@ -11,7 +11,7 @@ function get_dzhanibekov(; main_body = Capsule(radius, body_length, body_mass, color=color, name=:main) side_body = Capsule(0.5 * radius, 0.35 * body_length, 0.5 * body_mass, - axis_offset=UnitQuaternion(RotY(0.5 * π)), + axis_offset=RotY(0.5 * π), color=color, name=:side) links = [main_body, side_body] diff --git a/environments/humanoid/methods/initialize.jl b/environments/humanoid/methods/initialize.jl index ca0d530aa..eccfe520e 100644 --- a/environments/humanoid/methods/initialize.jl +++ b/environments/humanoid/methods/initialize.jl @@ -25,8 +25,8 @@ function get_humanoid(; aa = -0.43000 * [-0.44721, 0.00000, 0.89442] ql = axis_angle_to_quaternion(aa) - qll = ql * UnitQuaternion(RotXYZ(roll=-1.57080, pitch=1.47585, yaw=-1.47585)) # roll pitch yaw - qlr = ql * UnitQuaternion(RotXYZ(roll=+1.57080, pitch=1.47585, yaw=+1.47585)) # roll pitch yaw + qll = ql * RotX(-1.57080)*RotY(1.47585)*RotZ(-1.47585) # Quaternion(RotXYZ(roll=-1.57080, pitch=1.47585, yaw=-1.47585)) # roll pitch yaw + qlr = ql * RotX(+1.57080)*RotY(1.47585)*RotZ(+1.47585) # Quaternion(RotXYZ(roll=+1.57080, pitch=1.47585, yaw=+1.47585)) # roll pitch yaw pfll = vector_rotate([ 0.5 * left_foot.shape.shape[1].rh[2] + 0.03500; -0.03; 0.0], qll) pbll = vector_rotate([-0.5 * left_foot.shape.shape[1].rh[2] + 0.03500; -0.03; 0.0], qll) diff --git a/environments/orbital/methods/initialize.jl b/environments/orbital/methods/initialize.jl index c9f2ff079..8f21b4548 100644 --- a/environments/orbital/methods/initialize.jl +++ b/environments/orbital/methods/initialize.jl @@ -55,7 +55,7 @@ function initialize_orbital!(mechanism::Mechanism{T}; # set position and velocities set_maximal_configurations!(mechanism.origin, pbody, child_vertex=vert11, - Δq=UnitQuaternion(RotX(0.0))) + Δq=RotX(0.0)) set_minimal_coordinates!(mechanism, mechanism.joints[2], orientation) diff --git a/environments/pendulum/methods/initialize.jl b/environments/pendulum/methods/initialize.jl index d359a7774..808edb466 100644 --- a/environments/pendulum/methods/initialize.jl +++ b/environments/pendulum/methods/initialize.jl @@ -6,7 +6,7 @@ function get_pendulum(; spring=0.0, damper=0.0, spring_offset=szeros(1), - axis_offset=one(UnitQuaternion), + axis_offset=one(Quaternion), T=Float64) # Parameters @@ -106,7 +106,7 @@ function initialize_npendulum!(mechanism::Mechanism{T}; # set position and velocities set_maximal_configurations!(mechanism.origin, pbody, child_vertex=vert11, - Δq=UnitQuaternion(RotX(base_angle))) + Δq=RotX(base_angle)) set_maximal_velocities!(pbody, ω=base_angular_velocity) diff --git a/environments/raiberthopper/methods/env.jl b/environments/raiberthopper/methods/env.jl index db355ebc5..8c4cda337 100644 --- a/environments/raiberthopper/methods/env.jl +++ b/environments/raiberthopper/methods/env.jl @@ -142,7 +142,7 @@ function visualize(env::Environment{RaibertHopper}, traj::Vector{Vector{T}}; name=name) step = range(0.0, stop=norm(dir), length=n_leg) for i = 1:n_leg - MeshCat.settransform!(env.vis["leg$i"], MeshCat.compose(MeshCat.Translation(step[i] .* dir_norm + x_foot), MeshCat.LinearMap(Rotations.RotY(0.0)))) + MeshCat.settransform!(env.vis["leg$i"], MeshCat.compose(MeshCat.Translation(step[i] .* dir_norm + x_foot), MeshCat.LinearMap(Dojo.RotY(0.0)))) end end end diff --git a/environments/raiberthopper/methods/initialize.jl b/environments/raiberthopper/methods/initialize.jl index 3ce037409..793a29db0 100644 --- a/environments/raiberthopper/methods/initialize.jl +++ b/environments/raiberthopper/methods/initialize.jl @@ -91,7 +91,7 @@ function initialize_raiberthopper!(mech::Mechanism{T,Nn,Ne,Nb}; # body to foot set_maximal_configurations!(pbody, cbody, Δx=[0.0; 0.0; -leg_length], - Δq=UnitQuaternion(RotX(0.0))) + Δq=RotX(0.0)) set_maximal_velocities!(pbody, cbody, parent_vertex=tra2.vertices[1], child_vertex=tra2.vertices[2], diff --git a/environments/snake/methods/initialize.jl b/environments/snake/methods/initialize.jl index 8c77847d9..c873a8797 100644 --- a/environments/snake/methods/initialize.jl +++ b/environments/snake/methods/initialize.jl @@ -70,7 +70,7 @@ end function initialize_snake!(mechanism::Mechanism{T}; base_position=[0.0, -0.5, 0.0], - base_orientation=UnitQuaternion(RotX(0.6 * π)), + base_orientation=RotX(0.6 * π), base_linear_velocity=zeros(3), base_angular_velocity=zeros(3), relative_linear_velocity=zeros(3), diff --git a/environments/sphere/methods/initialize.jl b/environments/sphere/methods/initialize.jl index 419f9a659..d2669af46 100644 --- a/environments/sphere/methods/initialize.jl +++ b/environments/sphere/methods/initialize.jl @@ -33,7 +33,7 @@ end function initialize_sphere!(mechanism::Mechanism{T}; x=zeros(3), - q=one(UnitQuaternion), + q=one(Quaternion), v=zeros(3), ω=zeros(3)) where T diff --git a/environments/tennisracket/methods/initialize.jl b/environments/tennisracket/methods/initialize.jl index f8c53cf27..486ffb8d7 100644 --- a/environments/tennisracket/methods/initialize.jl +++ b/environments/tennisracket/methods/initialize.jl @@ -16,7 +16,7 @@ end function initialize_tennisracket!(mechanism::Mechanism{T}; x=zeros(3), - q=one(UnitQuaternion), + q=one(Quaternion), v=zeros(3), ω=zeros(3)) where T diff --git a/environments/twister/methods/initialize.jl b/environments/twister/methods/initialize.jl index fad0e58f4..2ecbdf542 100644 --- a/environments/twister/methods/initialize.jl +++ b/environments/twister/methods/initialize.jl @@ -69,7 +69,7 @@ end function initialize_twister!(mechanism::Mechanism{T}; base_position=[0.0, -1.0, 0.0], - base_orientation=UnitQuaternion(RotX(0.6 * π)), + base_orientation=RotX(0.6 * π), base_linear_velocity=zeros(3), base_angular_velocity=zeros(3), relative_linear_velocity=zeros(3), diff --git a/examples/animations/generate/box.jl b/examples/animations/generate/box.jl index efcfd119d..23b898a95 100644 --- a/examples/animations/generate/box.jl +++ b/examples/animations/generate/box.jl @@ -25,7 +25,7 @@ for t = 1:length(z_vis) u = (t == length(z_vis) ? 0.5 * u_vis[end] ./ u_max : 0.5 * u_vis[t] ./ u_max) MeshCat.atframe(anim, t) do - settransform!(v[:robot], MeshCat.compose(MeshCat.Translation(z[2], z[1], z[3]), MeshCat.LinearMap(UnitQuaternion(z[6 .+ (1:4)]...)))) + settransform!(v[:robot], MeshCat.compose(MeshCat.Translation(z[2], z[1], z[3]), MeshCat.LinearMap(Quaternion(z[6 .+ (1:4)]...)))) settransform!(force_vis, Point(z[2] - u[2] - shift[2], z[1] - u[1] - shift[1], z[3] - u[3] - shift[3]), Vec(u[2], u[1], u[3]), diff --git a/examples/simulation/cone_compare.jl b/examples/simulation/cone_compare.jl index d106dee88..bf8e8a5dc 100644 --- a/examples/simulation/cone_compare.jl +++ b/examples/simulation/cone_compare.jl @@ -43,7 +43,7 @@ mech_lc = get_mechanism(:box, # ## Simulate initialize!(mech_lc, :box, x=x0, - q=one(UnitQuaternion), + q=one(Quaternion), v=v0, ω=ω0) @@ -70,7 +70,7 @@ mech_nc = get_mechanism(:box, # ## Simulate initialize!(mech_nc, :box, x=x0, - q=one(UnitQuaternion), + q=one(Quaternion), v=v0, ω=ω0) @@ -101,7 +101,7 @@ mech_mjlc = get_mechanism(:box, # ## Load initialize!(mech_mjlc, :box, x=x0, - q=one(UnitQuaternion), + q=one(Quaternion), v=v0, ω=ω0) file = jldopen(joinpath(@__DIR__, "../mujoco_benchmark/results/cone_compare_pyramidal.jld2")) @@ -135,7 +135,7 @@ mech_mjnc = get_mechanism(:box, # ## Load initialize!(mech_mjnc, :box, x=x0, - q=one(UnitQuaternion), + q=one(Quaternion), v=v0, ω=ω0) file = jldopen(joinpath(@__DIR__, "../mujoco_benchmark/results/cone_compare_elliptic.jld2")) diff --git a/examples/system_identification/data/data.jl b/examples/system_identification/data/data.jl index b8f70fb6c..ea007af9d 100644 --- a/examples/system_identification/data/data.jl +++ b/examples/system_identification/data/data.jl @@ -33,8 +33,8 @@ function toss2z(toss, timestep; s=1) q2 = vec2[4:7] v15 = (x2 - x1) / timestep - ϕ15 = ω_finite_difference(UnitQuaternion(q1...), - UnitQuaternion(q2...), timestep) + ϕ15 = ω_finite_difference(Quaternion(q1...), + Quaternion(q2...), timestep) z2 = [x2; v15; q2; ϕ15] push!(z, z2) diff --git a/examples/system_identification/data/dataset/box2d_dim_N:15_cf:0.1_radius:0.05_side:0.5.jld2 b/examples/system_identification/data/dataset/box2d_dim_N_15_cf_0.1_radius_0.05_side_0.5.jld2 similarity index 100% rename from examples/system_identification/data/dataset/box2d_dim_N:15_cf:0.1_radius:0.05_side:0.5.jld2 rename to examples/system_identification/data/dataset/box2d_dim_N_15_cf_0.1_radius_0.05_side_0.5.jld2 diff --git a/examples/system_identification/data/dataset/box_dim_N:35_cf:0.1_radius:0.0_side:0.5.jld2 b/examples/system_identification/data/dataset/box_dim_N_35_cf_0.1_radius_0.0_side_0.5.jld2 similarity index 100% rename from examples/system_identification/data/dataset/box_dim_N:35_cf:0.1_radius:0.0_side:0.5.jld2 rename to examples/system_identification/data/dataset/box_dim_N_35_cf_0.1_radius_0.0_side_0.5.jld2 diff --git a/examples/system_identification/data/dataset/hardwarebox_dim_N:400_sample_S:1.jld2 b/examples/system_identification/data/dataset/hardwarebox_dim_N_400_sample_S_1.jld2 similarity index 100% rename from examples/system_identification/data/dataset/hardwarebox_dim_N:400_sample_S:1.jld2 rename to examples/system_identification/data/dataset/hardwarebox_dim_N_400_sample_S_1.jld2 diff --git a/examples/system_identification/data/dataset/hardwarebox_dim_N:400_sample_S:7.jld2 b/examples/system_identification/data/dataset/hardwarebox_dim_N_400_sample_S_7.jld2 similarity index 100% rename from examples/system_identification/data/dataset/hardwarebox_dim_N:400_sample_S:7.jld2 rename to examples/system_identification/data/dataset/hardwarebox_dim_N_400_sample_S_7.jld2 diff --git a/examples/system_identification/data/dataset/sphere_dim_N:15_cf:0.1_radius:0.5.jld2 b/examples/system_identification/data/dataset/sphere_dim_N_15_cf_0.1_radius_0.5.jld2 similarity index 100% rename from examples/system_identification/data/dataset/sphere_dim_N:15_cf:0.1_radius:0.5.jld2 rename to examples/system_identification/data/dataset/sphere_dim_N_15_cf_0.1_radius_0.5.jld2 diff --git a/examples/system_identification/methods/utils.jl b/examples/system_identification/methods/utils.jl index b8c17774e..b56ae31e4 100644 --- a/examples/system_identification/methods/utils.jl +++ b/examples/system_identification/methods/utils.jl @@ -128,19 +128,19 @@ function datafilename(model::Symbol; kwargs...) end function datafilenamesphere(; N::Int=10, friction_coefficient=0.1, radius=0.5) - "sphere_dim_N:$(N)_friction_coefficient:$(friction_coefficient)_radius:$(radius).jld2" + "sphere_dim_N_$(N)_friction_coefficient_$(friction_coefficient)_radius_$(radius).jld2" end function datafilenamebox2D(; N::Int=10, friction_coefficient=0.1, radius=0.05, side=0.50) - "box2D_dim_N:$(N)_friction_coefficient:$(friction_coefficient)_radius:$(radius)_side:$(side).jld2" + "box2D_dim_N_$(N)_friction_coefficient_$(friction_coefficient)_radius_$(radius)_side_$(side).jld2" end function datafilenamebox(; N::Int=10, friction_coefficient=0.1, radius=0., side=0.50) - "box_dim_N:$(N)_friction_coefficient:$(friction_coefficient)_radius:$(radius)_side:$(side).jld2" + "box_dim_N_$(N)_friction_coefficient_$(friction_coefficient)_radius_$(radius)_side_$(side).jld2" end function datafilenamehardwarebox(; N::Int=10, s=1) - "hardwarebox_dim_N:$(N)_sample_S:$(S).jld2" + "hardwarebox_dim_N_$(N)_sample_S_$(S).jld2" end function initial_state(model::Symbol; kwargs...) @@ -176,7 +176,7 @@ function initial_state_box(; x = xlims[1] + rand(3) .* (xlims[2] - xlims[1]) v = vlims[1] + rand(3) .* (vlims[2] - vlims[1]) ω = ωlims[1] + rand(3) .* (ωlims[2] - ωlims[1]) - return Dict(:x => x, :v => v , :q => rand(UnitQuaternion), :ω => ω) + return Dict(:x => x, :v => v , :q => normalize(rand(Quaternion{Float64})), :ω => ω) end function build_pairs(mechanism::Mechanism, trajs::AbstractVector) diff --git a/src/Dojo.jl b/src/Dojo.jl index 0f3c4d97a..6cd6edd57 100644 --- a/src/Dojo.jl +++ b/src/Dojo.jl @@ -8,8 +8,7 @@ using Random using StaticArrays using SparseArrays using StaticArrays: SUnitRange -using Rotations -using Rotations: RotationError, params, lmult, rmult, tmat, vmat, hmat, skew, pure_quaternion +using Quaternions using Parameters using Statistics @@ -256,10 +255,7 @@ export # Orientation export - UnitQuaternion, - RotX, - RotY, - RotZ, + Quaternion, attitude_jacobian # Data diff --git a/src/bodies/set.jl b/src/bodies/set.jl index aaec7136c..b814cdd49 100644 --- a/src/bodies/set.jl +++ b/src/bodies/set.jl @@ -38,7 +38,7 @@ end # maximal function set_maximal_configurations!(body::Body; x::AbstractVector=SA[0.0; 0.0; 0.0], - q::UnitQuaternion=one(UnitQuaternion)) + q::Quaternion=one(Quaternion)) body.state.x2 = x body.state.q2 = q @@ -60,7 +60,7 @@ function set_maximal_configurations!(pbody::Node, cbody::Body; parent_vertex::AbstractVector=SA[0.0; 0.0; 0.0], child_vertex::AbstractVector=SA[0.0; 0.0; 0.0], Δx::AbstractVector=SA[0.0; 0.0; 0.0], - Δq::UnitQuaternion=one(UnitQuaternion)) + Δq::Quaternion=one(Quaternion)) q1 = pbody.state.q2 q2 = pbody.state.q2 * Δq diff --git a/src/bodies/shapes.jl b/src/bodies/shapes.jl index fdb1e7900..3e8f52a36 100644 --- a/src/bodies/shapes.jl +++ b/src/bodies/shapes.jl @@ -23,7 +23,7 @@ end """ mutable struct Mesh{T} <: Shape{T} position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} path::String scale::SVector{3,T} @@ -31,20 +31,20 @@ mutable struct Mesh{T} <: Shape{T} function Mesh(path::String; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((position_offset, axis_offset))...) + T = promote_type(quateltype.((position_offset, axis_offset))...) new{T}(position_offset, axis_offset, path, scale, color) end function Mesh(path::String, m::Real, J::AbstractMatrix; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((m, J, position_offset, axis_offset))...) + T = promote_type(quateltype.((m, J, position_offset, axis_offset))...) return Body(m, J; name=name, shape=new{T}(position_offset, axis_offset, path, scale, color)) end end @@ -62,7 +62,7 @@ end """ mutable struct Box{T} <: Shape{T} position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} xyz::SVector{3,T} scale::SVector{3,T} @@ -70,20 +70,20 @@ mutable struct Box{T} <: Shape{T} function Box(x::Real, y::Real, z::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((x, y, z, position_offset, axis_offset))...) + T = promote_type(quateltype.((x, y, z, position_offset, axis_offset))...) new{T}(position_offset, axis_offset, [x; y; z], scale, color) end function Box(x::Real, y::Real, z::Real, m::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((x, y, z, m, position_offset, axis_offset))...) + T = promote_type(quateltype.((x, y, z, m, position_offset, axis_offset))...) J = 1 / 12 * m * diagm([y^2 + z^2; x^2 + z^2; x^2 + y^2]) return Body(m, J; name=name, shape=new{T}(position_offset, axis_offset, [x;y;z], scale, color)) end @@ -102,7 +102,7 @@ end """ mutable struct Cylinder{T} <: Shape{T} position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} rh::SVector{2,T} scale::SVector{3,T} @@ -111,20 +111,20 @@ mutable struct Cylinder{T} <: Shape{T} # Cylinder points in the z direction function Cylinder(r::Real, h::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((r, h, position_offset, axis_offset))...) + T = promote_type(quateltype.((r, h, position_offset, axis_offset))...) new{T}(position_offset, axis_offset, [r;h], scale, color) end function Cylinder(r::Real, h::Real, m::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((r, h, m, position_offset, axis_offset))...) + T = promote_type(quateltype.((r, h, m, position_offset, axis_offset))...) J = 1 / 2 * m * diagm([r^2 + 1 / 6 * h^2; r^2 + 1 / 6 * h^2; r^2]) return Body(m, J; name=name, shape=new{T}(position_offset, axis_offset, [r;h], scale, color)) end @@ -143,7 +143,7 @@ end """ mutable struct Capsule{T} <: Shape{T} position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} rh::SVector{2,T} scale::SVector{3,T} @@ -152,20 +152,20 @@ mutable struct Capsule{T} <: Shape{T} # Capsule points in the z direction function Capsule(r::Real, h::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion= one(UnitQuaternion), + axis_offset::Quaternion= one(Quaternion), scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((r, h, position_offset, axis_offset))...) + T = promote_type(quateltype.((r, h, position_offset, axis_offset))...) new{T}(position_offset, axis_offset, [r; h], scale, color) end function Capsule(r::Real, h::Real, m::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((r, h, m, position_offset, axis_offset))...) + T = promote_type(quateltype.((r, h, m, position_offset, axis_offset))...) mass_cylinder = π * h * r^2.0 mass_hemisphere = π * 2.0 / 3.0 * r^3.0 @@ -197,13 +197,13 @@ end mutable struct Shapes{T} <: Shape{T} shape::Vector position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} scale::SVector{3,T} color::RGBA function Shapes(shapes::Vector{Shape{T}}; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) where T @@ -212,7 +212,7 @@ mutable struct Shapes{T} <: Shape{T} function Shapes(shapes::Vector, m::T, J; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) where T @@ -233,27 +233,27 @@ end """ mutable struct Sphere{T} <: Shape{T} position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} r::T scale::SVector{3,T} color::RGBA function Sphere(r::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((r, position_offset, axis_offset))...) + T = promote_type(quateltype.((r, position_offset, axis_offset))...) new{T}(position_offset, axis_offset, r, scale, color) end function Sphere(r::Real, m::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((r, m, position_offset, axis_offset))...) + T = promote_type(quateltype.((r, m, position_offset, axis_offset))...) J = 2 / 5 * m * diagm([r^2 for i = 1:3]) return Body(m, J; name=name, shape=new{T}(position_offset, axis_offset, r, scale, color)) end @@ -272,7 +272,7 @@ end """ mutable struct Pyramid{T} <: Shape{T} position_offset::SVector{3,T} - axis_offset::UnitQuaternion{T} + axis_offset::Quaternion{T} wh::SVector{2,T} scale::SVector{3,T} color::RGBA @@ -280,19 +280,19 @@ mutable struct Pyramid{T} <: Shape{T} # Pyramid points in the z direction, Center of mass at 1/4 h function Pyramid(w::Real, h::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((w, h, position_offset, axis_offset))...) + T = promote_type(quateltype.((w, h, position_offset, axis_offset))...) new{T}(position_offset, axis_offset, [w;h], scale, color) end function Pyramid(w::Real, h::Real, m::Real; position_offset::AbstractVector=szeros(3), - axis_offset::UnitQuaternion=one(UnitQuaternion), + axis_offset::Quaternion=one(Quaternion), scale::AbstractVector=sones(3), name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75)) - T = promote_type(eltype.((w, h, m, position_offset, axis_offset))...) + T = promote_type(quateltype.((w, h, m, position_offset, axis_offset))...) J = 1/80 * m * diagm([4*w^2+3*h^2;4*w^2+3*h^2;8*w^2]) return Body(m, J; name=name, shape=new{T}(position_offset, axis_offset, [w;h], scale, color)) end diff --git a/src/bodies/state.jl b/src/bodies/state.jl index 94b362292..bc245a5c8 100644 --- a/src/bodies/state.jl +++ b/src/bodies/state.jl @@ -5,12 +5,12 @@ information at time step 3 is recovered using configurations at time step 2 and velocities at time step 2.5. x1: position at previous time step - q1: orientation (UnitQuaternion) at previous time step + q1: orientation (Quaternion) at previous time step v15: linear velocity at time step 1.5 (midpoint) ϕ15: angular velocity at time step 1.5 (midpoint) x2: position at current time step - q2: orientation (UnitQuaternion) at current time step + q2: orientation (Quaternion) at current time step F2: linear impulse (force * time step) applied at current time step τ2: angular impulse (torque * timestep) applied at current time step @@ -23,13 +23,13 @@ mutable struct State{T} # previous state x1::SVector{3,T} - q1::UnitQuaternion{T} + q1::Quaternion{T} v15::SVector{3,T} ϕ15::SVector{3,T} # current state x2::SVector{3,T} - q2::UnitQuaternion{T} + q2::Quaternion{T} F2::SVector{3,T} τ2::SVector{3,T} @@ -43,12 +43,12 @@ mutable struct State{T} function State{T}() where T x1 = szeros(T, 3) - q1 = one(UnitQuaternion{T}) + q1 = one(Quaternion{T}) v15 = szeros(T, 3) ϕ15 = szeros(T, 3) x2 = szeros(T, 3) - q2 = one(UnitQuaternion{T}) + q2 = one(Quaternion{T}) F2 = szeros(T, 3) τ2 = szeros(T, 3) diff --git a/src/contacts/contact.jl b/src/contacts/contact.jl index 57d86fa24..ddc7525db 100644 --- a/src/contacts/contact.jl +++ b/src/contacts/contact.jl @@ -15,14 +15,15 @@ constraint_jacobian_velocity(model::Contact, body::Body, id, λ, timestep) = con # impulses impulse_map(model::Contact, body::Body, id, λ, timestep) = impulse_map(model, next_configuration(body.state, timestep)..., λ) -function impulse_map(model::Contact, x::AbstractVector, q::UnitQuaternion, λ) +function impulse_map(model::Contact, x::AbstractVector, q::Quaternion, λ) X = force_mapping(model, x, q) - Q = - X * q * skew(model.contact_point - vector_rotate(model.offset, inv(q))) + # Q = - X * q * skew(model.contact_point - vector_rotate(model.offset, inv(q))) + Q = - X * rotation_matrix(q) * skew(model.contact_point - vector_rotate(model.offset, inv(q))) return [X'; Q'] end # force mapping -function force_mapping(model::Contact, x::AbstractVector, q::UnitQuaternion) +function force_mapping(model::Contact, x::AbstractVector, q::Quaternion) X = [model.surface_normal_projector; szeros(1,3); model.surface_projector] diff --git a/src/contacts/impact.jl b/src/contacts/impact.jl index 1adecb8fa..a2960a3a4 100644 --- a/src/contacts/impact.jl +++ b/src/contacts/impact.jl @@ -30,23 +30,23 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N SVector{1,T}(model.surface_normal_projector * (x3 + vector_rotate(model.contact_point,q3) - model.offset) - contact.impulses_dual[2][1]) end -function constraint_jacobian_configuration(model::ImpactContact, x3::AbstractVector, q3::UnitQuaternion, - x2::AbstractVector, v25::AbstractVector, q2::UnitQuaternion, ϕ25::AbstractVector, +function constraint_jacobian_configuration(model::ImpactContact, x3::AbstractVector, q3::Quaternion, + x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ϕ25::AbstractVector, λ, timestep) X = model.surface_normal_projector Q = model.surface_normal_projector * ∂vector_rotate∂q(model.contact_point, q3) return [X Q] end -function constraint_jacobian_velocity(model::ImpactContact, x3::AbstractVector, q3::UnitQuaternion, - x2::AbstractVector, v25::AbstractVector, q2::UnitQuaternion, ϕ25::AbstractVector, +function constraint_jacobian_velocity(model::ImpactContact, x3::AbstractVector, q3::Quaternion, + x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ϕ25::AbstractVector, λ, timestep) V = model.surface_normal_projector * timestep Ω = model.surface_normal_projector * ∂vector_rotate∂q(model.contact_point, q3) * rotational_integrator_jacobian_velocity(q2, ϕ25, timestep) return [V Ω] end -function force_mapping(model::ImpactContact, x::AbstractVector, q::UnitQuaternion) +function force_mapping(model::ImpactContact, x::AbstractVector, q::Quaternion) X = model.surface_normal_projector return X end diff --git a/src/contacts/linear.jl b/src/contacts/linear.jl index 41a661a38..2969674ae 100644 --- a/src/contacts/linear.jl +++ b/src/contacts/linear.jl @@ -54,8 +54,8 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N (model.surface_projector * vp + ψ * sones(4) - sβ)...) end -function constraint_jacobian_configuration(model::LinearContact, x3::AbstractVector, q3::UnitQuaternion, - x2::AbstractVector, v25::AbstractVector, q2::UnitQuaternion, ϕ25::AbstractVector, +function constraint_jacobian_configuration(model::LinearContact, x3::AbstractVector, q3::Quaternion, + x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ϕ25::AbstractVector, λ, timestep) V = [model.surface_normal_projector; szeros(1,3); @@ -68,8 +68,8 @@ function constraint_jacobian_configuration(model::LinearContact, x3::AbstractVec return [V Ω] end -function constraint_jacobian_velocity(model::LinearContact, x3::AbstractVector, q3::UnitQuaternion, - x2::AbstractVector, v25::AbstractVector, q2::UnitQuaternion, ϕ25::AbstractVector, +function constraint_jacobian_velocity(model::LinearContact, x3::AbstractVector, q3::Quaternion, + x2::AbstractVector, v25::AbstractVector, q2::Quaternion, ϕ25::AbstractVector, λ, timestep) V = [model.surface_normal_projector * timestep; szeros(1,3); diff --git a/src/contacts/nonlinear.jl b/src/contacts/nonlinear.jl index 1709f5669..b768ff919 100644 --- a/src/contacts/nonlinear.jl +++ b/src/contacts/nonlinear.jl @@ -41,7 +41,7 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs}) where {T,N end function constraint(model::NonlinearContact, s::AbstractVector{T}, γ::AbstractVector{T}, - x3::AbstractVector{T}, q3::UnitQuaternion{T}, + x3::AbstractVector{T}, q3::Quaternion{T}, v25::AbstractVector{T}, ϕ25::AbstractVector{T}) where T # transforms the velocities of the origin of the link into velocities @@ -53,8 +53,8 @@ function constraint(model::NonlinearContact, s::AbstractVector{T}, γ::AbstractV end function constraint_jacobian_velocity(model::NonlinearContact{T}, - x3::AbstractVector{T}, q3::UnitQuaternion{T}, - x2::AbstractVector{T}, v25::AbstractVector{T}, q2::UnitQuaternion{T}, ϕ25::AbstractVector{T}, + x3::AbstractVector{T}, q3::Quaternion{T}, + x2::AbstractVector{T}, v25::AbstractVector{T}, q2::Quaternion{T}, ϕ25::AbstractVector{T}, λ, timestep::T) where T V = [model.surface_normal_projector * timestep; szeros(1,3); @@ -70,8 +70,8 @@ function constraint_jacobian_velocity(model::NonlinearContact{T}, end function constraint_jacobian_configuration(model::NonlinearContact{T}, - x3::AbstractVector{T}, q3::UnitQuaternion{T}, - x2::AbstractVector{T}, v25::AbstractVector{T}, q2::UnitQuaternion{T}, ϕ25::AbstractVector{T}, + x3::AbstractVector{T}, q3::Quaternion{T}, + x2::AbstractVector{T}, v25::AbstractVector{T}, q2::Quaternion{T}, ϕ25::AbstractVector{T}, λ, timestep::T) where T X = [model.surface_normal_projector; szeros(1,3); diff --git a/src/contacts/utilities.jl b/src/contacts/utilities.jl index 824ab27fb..42663daca 100644 --- a/src/contacts/utilities.jl +++ b/src/contacts/utilities.jl @@ -8,7 +8,7 @@ q: body orientation """ function get_sdf(contact::ContactConstraint{T,N,Nc,Cs}, x::AbstractVector{T}, - q::UnitQuaternion{T}) where {T,N,Nc,Cs<:Contact{T,N}} + q::Quaternion{T}) where {T,N,Nc,Cs<:Contact{T,N}} model = contact.model return model.surface_normal_projector * (x + vector_rotate(model.contact_point, q) - model.offset) end @@ -32,7 +32,7 @@ end q: body orientation """ function contact_location(contact::ContactConstraint{T,N,Nc,Cs}, x::AbstractVector{T}, - q::UnitQuaternion{T}) where {T,N,Nc,Cs<:Contact{T,N}} + q::Quaternion{T}) where {T,N,Nc,Cs<:Contact{T,N}} model = contact.model return x + vector_rotate(model.contact_point,q) - model.offset end diff --git a/src/gradients/utilities.jl b/src/gradients/utilities.jl index 52371ece8..7bab9987c 100644 --- a/src/gradients/utilities.jl +++ b/src/gradients/utilities.jl @@ -24,7 +24,7 @@ function attitude_jacobian(data::AbstractVector, Nb::Int) G = zeros(0,0) for i = 1:Nb x2, v15, q2, ϕ15 = unpack_data(data[13 * (i-1) .+ (1:13)]) - q2 = UnitQuaternion(q2..., false) + q2 = Quaternion(q2...) G = cat(G, I(6), LVᵀmat(q2), I(3), dims = (1,2)) end ndata = length(data) diff --git a/src/integrators/integrator.jl b/src/integrators/integrator.jl index 09517446b..7b9bf9c03 100644 --- a/src/integrators/integrator.jl +++ b/src/integrators/integrator.jl @@ -12,33 +12,33 @@ initial_configuration_velocity(state::State) = (current_position(state), state.v # next next_position(x2::SVector{3,T}, v25::SVector{3,T}, timestep::T) where T = x2 + v25 * timestep -next_orientation(q2::UnitQuaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T = q2 * quaternion_map(ϕ25, timestep) * timestep / 2 +next_orientation(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T = q2 * quaternion_map(ϕ25, timestep) * timestep / 2 next_position(state::State, timestep) = next_position(state.x2, state.vsol[2], timestep) next_orientation(state::State, timestep) = next_orientation(state.q2, state.ϕsol[2], timestep) next_configuration(state::State, timestep) = (next_position(state, timestep), next_orientation(state, timestep)) next_configuration_velocity(state::State, timestep) = (next_position(state, timestep), state.vsol[2], next_orientation(state, timestep), state.ϕsol[2]) # angular velocity -function angular_velocity(q1::UnitQuaternion, q2::UnitQuaternion, timestep) +function angular_velocity(q1::Quaternion, q2::Quaternion, timestep) 2.0 / timestep * Vmat() * Lᵀmat(q1) * vector(q2) end -function ∂angular_velocity∂q1(q1::UnitQuaternion, q2::UnitQuaternion, timestep) +function ∂angular_velocity∂q1(q1::Quaternion, q2::Quaternion, timestep) 2.0 / timestep * Vmat() * Rmat(q2) * Tmat() end -function ∂angular_velocity∂q2(q1::UnitQuaternion, q2::UnitQuaternion, timestep) +function ∂angular_velocity∂q2(q1::Quaternion, q2::Quaternion, timestep) 2.0 / timestep * Vmat() * Lᵀmat(q1) end # Jacobians -function integrator_jacobian_velocity(q2::UnitQuaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T +function integrator_jacobian_velocity(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T V = [linear_integrator_jacobian_velocity(timestep) szeros(T,3,3)] Ω = [szeros(T,4,3) rotational_integrator_jacobian_velocity(q2, ϕ25, timestep)] return [V; Ω] # 7x6 end -function integrator_jacobian_configuration(q2::UnitQuaternion{T}, ϕ25::SVector{3,T}, timestep::T; +function integrator_jacobian_configuration(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T; attjac::Bool=true) where T Z = attjac ? szeros(T,3,3) : szeros(T,3,4) @@ -55,14 +55,14 @@ function linear_integrator_jacobian_velocity(timestep::T) where T return timestep * SMatrix{3,3,T,9}(Diagonal(sones(T,3))) end -function rotational_integrator_jacobian_orientation(q2::UnitQuaternion{T}, ϕ25::SVector{3,T}, timestep::T; +function rotational_integrator_jacobian_orientation(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T; attjac::Bool = true) where T M = Rmat(quaternion_map(ϕ25, timestep) * timestep / 2) attjac && (M *= LVᵀmat(q2)) return M end -function rotational_integrator_jacobian_velocity(q2::UnitQuaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T +function rotational_integrator_jacobian_velocity(q2::Quaternion{T}, ϕ25::SVector{3,T}, timestep::T) where T return Lmat(q2) * quaternion_map_jacobian(ϕ25, timestep) * timestep / 2 end diff --git a/src/joints/impulses.jl b/src/joints/impulses.jl index f96205783..19094580d 100644 --- a/src/joints/impulses.jl +++ b/src/joints/impulses.jl @@ -2,7 +2,7 @@ # Impulse Transform ################################################################################ function impulse_transform(relative::Symbol, joint::Joint, xa::AbstractVector, - qa::UnitQuaternion, xb::AbstractVector, qb::UnitQuaternion) where {T} + qa::Quaternion, xb::AbstractVector, qb::Quaternion) where {T} X, Q = displacement_jacobian_configuration(relative, joint, xa, qa, xb, qb, attjac=true) Diagonal([sones(3); 0.5 * sones(3)]) * transpose([X Q]) end diff --git a/src/joints/joint.jl b/src/joints/joint.jl index 0a9d9462a..401c0e374 100644 --- a/src/joints/joint.jl +++ b/src/joints/joint.jl @@ -7,21 +7,21 @@ abstract type Joint{T,Nλ,Nb,N,Nb½} end # joints function joint_constraint(joint::Joint{T}, - xa::AbstractVector, qa::UnitQuaternion, xb::AbstractVector, qb::UnitQuaternion, η) where {T} + xa::AbstractVector, qa::Quaternion, xb::AbstractVector, qb::Quaternion, η) where {T} return constraint_mask(joint) * displacement(joint, xa, qa, xb, qb) end function joint_constraint_jacobian_configuration(relative::Symbol, joint::Joint{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, η) where {T} + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η) where {T} X, Q = displacement_jacobian_configuration(relative, joint, xa, qa, xb, qb, attjac=false) return constraint_mask(joint) * [X Q] end # constraints function constraint(joint::Joint{T,Nλ,0}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, η, μ) where {T,Nλ} + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η, μ) where {T,Nλ} joint_constraint(joint, xa, qa, xb, qb, η) end @@ -43,8 +43,8 @@ end constraint_jacobian_configuration(relative::Symbol, joint::Joint, pbody::Node, cbody::Node, λ, timestep) = constraint_jacobian_configuration(relative, joint, next_configuration(pbody.state, timestep)..., next_configuration(cbody.state, timestep)..., λ) function constraint_jacobian_configuration(relative::Symbol, joint::Joint{T,Nλ,0}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, η) where {T,Nλ} + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η) where {T,Nλ} joint_constraint_jacobian_configuration(relative, joint, xa, qa, xb, qb, η) end @@ -63,15 +63,15 @@ nullspace_mask(::Joint{T,3}) where T = szeros(T,0,3) impulse_map(relative::Symbol, joint::Joint, pbody::Node, cbody::Node, λ) = impulse_map(relative, joint, current_configuration(pbody.state)..., current_configuration(cbody.state)..., λ) function impulse_map(relative::Symbol, joint::Joint{T,Nλ,Nb}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η) where {T,Nλ,Nb} return impulse_transform(relative, joint, xa, qa, xb, qb) * impulse_projector(joint) end function impulse_map(relative::Symbol, joint::Joint{T,Nλ,0}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η) where {T,Nλ} J = constraint_jacobian_configuration(relative, joint, xa, qa, xb, qb, η) G = cat(Diagonal(sones(3)), LVᵀmat(relative == :parent ? qa : qb), dims=(1,2)) diff --git a/src/joints/limits.jl b/src/joints/limits.jl index 51066d17f..201e3a3d1 100644 --- a/src/joints/limits.jl +++ b/src/joints/limits.jl @@ -1,6 +1,6 @@ function constraint(joint::Joint{T,Nλ,Nb,N,Nb½}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η, μ) where {T,Nλ,Nb,N,Nb½} e1 = joint_constraint(joint, xa, qa, xb, qb, η) @@ -17,8 +17,8 @@ function constraint(joint::Joint{T,Nλ,Nb,N,Nb½}, end function constraint_jacobian_configuration(relative::Symbol, joint::Joint{T,Nλ,Nb}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, η) where {T,Nλ,Nb} ∇comp = szeros(T,Nb,7) diff --git a/src/joints/minimal.jl b/src/joints/minimal.jl index 05102f79e..4ad606bbe 100644 --- a/src/joints/minimal.jl +++ b/src/joints/minimal.jl @@ -84,8 +84,8 @@ function set_minimal_velocities!(mechanism, joint::JointConstraint{T,N,Nc}, vϕ) end function get_child_velocity(joint::JointConstraint, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, qb::Quaternion, timestep; Δv=szeros(input_dimension(joint.translational)), Δϕ=szeros(input_dimension(joint.rotational))) @@ -169,7 +169,7 @@ function set_minimal_coordinates_velocities!(joint::JointConstraint, # parent state xa = SVector{3}(zp[1:3]) va = SVector{3}(zp[3 .+ (1:3)]) - qa = UnitQuaternion(zp[6 .+ (1:4)]..., false) + qa = Quaternion(zp[6 .+ (1:4)]...) ϕa = SVector{3}(zp[10 .+ (1:3)]) # positions @@ -202,7 +202,7 @@ function set_minimal_coordinates_velocities!(joint::JointConstraint, end function minimal_coordinates_velocities_jacobian_parent(joint::JointConstraint{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, timestep; Δx=szeros(input_dimension(joint.translational)), Δθ=szeros(input_dimension(joint.rotational)), @@ -307,7 +307,7 @@ function set_minimal_coordinates_velocities_jacobian_parent(joint::JointConstrai end function minimal_coordinates_velocities_jacobian_minimal(joint::JointConstraint{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, timestep; Δx=szeros(input_dimension(joint.translational)), Δθ=szeros(input_dimension(joint.rotational)), diff --git a/src/joints/prototypes.jl b/src/joints/prototypes.jl index 9729af91f..f97c75a5a 100644 --- a/src/joints/prototypes.jl +++ b/src/joints/prototypes.jl @@ -6,7 +6,7 @@ Fixed(pbody::Node{T}, cbody::Node{T}; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T})) where T = + axis_offset=one(Quaternion{T})) where T = Translational{T,3}(pbody, cbody; parent_vertex, child_vertex), @@ -21,7 +21,7 @@ Fixed(pbody::Node{T}, cbody::Node{T}; Prismatic(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,1), @@ -47,7 +47,7 @@ Prismatic(pbody::Node{T}, cbody::Node{T}, axis; Planar(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,2), @@ -71,7 +71,7 @@ Planar(pbody::Node{T}, cbody::Node{T}, axis; three translational degree of freedom between two bodies """ FixedOrientation(pbody::Node{T}, cbody::Node{T}; - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,3), @@ -94,7 +94,7 @@ FixedOrientation(pbody::Node{T}, cbody::Node{T}; Revolute(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), rot_spring_offset=szeros(T,1), @@ -122,7 +122,7 @@ Revolute(pbody::Node{T}, cbody::Node{T}, axis; Cylindrical(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,1), @@ -155,7 +155,7 @@ Cylindrical(pbody::Node{T}, cbody::Node{T}, axis; PlanarAxis(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,2), @@ -188,7 +188,7 @@ PlanarAxis(pbody::Node{T}, cbody::Node{T}, axis; FreeRevolute(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,3), @@ -218,7 +218,7 @@ FreeRevolute(pbody::Node{T}, cbody::Node{T}, axis; Orbital(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), rot_spring_offset=szeros(T,2), @@ -247,7 +247,7 @@ Orbital(pbody::Node{T}, cbody::Node{T}, axis; PrismaticOrbital(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,1), @@ -280,7 +280,7 @@ PrismaticOrbital(pbody::Node{T}, cbody::Node{T}, axis; PlanarOrbital(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,2), @@ -313,7 +313,7 @@ PlanarOrbital(pbody::Node{T}, cbody::Node{T}, axis; FreeOrbital(pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=szeros(T,3), @@ -343,7 +343,7 @@ FreeOrbital(pbody::Node{T}, cbody::Node{T}, axis; Spherical(pbody::Node{T}, cbody::Node{T}; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), rot_spring_offset=szeros(T,3), @@ -449,7 +449,7 @@ Floating(pbody::Node{T}, cbody::Node{T}; function Prototype(joint_type::Symbol, pbody::Node{T}, cbody::Node{T}, axis; parent_vertex=szeros(T, 3), child_vertex=szeros(T, 3), - axis_offset=one(UnitQuaternion{T}), + axis_offset=one(Quaternion{T}), spring=zero(T), damper=zero(T), tra_spring_offset=nothing, diff --git a/src/joints/rotational/constructor.jl b/src/joints/rotational/constructor.jl index f1c18ecd0..5216bcf3b 100644 --- a/src/joints/rotational/constructor.jl +++ b/src/joints/rotational/constructor.jl @@ -20,7 +20,7 @@ mutable struct Rotational{T,Nλ,Nb,N,Nb½,N̄λ} <: Joint{T,Nλ,Nb,N,Nb½} axis_mask1::Adjoint{T,SVector{3,T}} # in pbody's frame axis_mask2::Adjoint{T,SVector{3,T}} # in pbody's frame axis_mask3::Adjoint{T,SVector{3,T}} # in pbody's frame - axis_offset::UnitQuaternion{T} # in pbody's frame + axis_offset::Quaternion{T} # in pbody's frame spring::T damper::T spring_offset::SVector{N̄λ,T} @@ -31,7 +31,7 @@ end function Rotational{T,Nλ}(pbody::Node, cbody::Node; axis::AbstractVector=szeros(T,3), - axis_offset::UnitQuaternion=one(UnitQuaternion{T}), + axis_offset::Quaternion=one(Quaternion{T}), spring=zero(T), damper=zero(T), spring_offset=szeros(T,3-Nλ), diff --git a/src/joints/rotational/dampers.jl b/src/joints/rotational/dampers.jl index f78f196e2..6541c5e1c 100644 --- a/src/joints/rotational/dampers.jl +++ b/src/joints/rotational/dampers.jl @@ -2,8 +2,8 @@ # Damper Force ############################################################################### -function damper_force(joint::Rotational{T}, qa::UnitQuaternion, ϕa::AbstractVector, - qb::UnitQuaternion, ϕb::AbstractVector, +function damper_force(joint::Rotational{T}, qa::Quaternion, ϕa::AbstractVector, + qb::Quaternion, ϕb::AbstractVector, timestep; unitary::Bool=false) where T A = nullspace_mask(joint) @@ -14,8 +14,8 @@ function damper_force(joint::Rotational{T}, qa::UnitQuaternion, ϕa::AbstractVec return force # in the offset frame end -function damper_force(relative::Symbol, joint::Rotational{T}, qa::UnitQuaternion, ϕa::AbstractVector, - qb::UnitQuaternion, ϕb::AbstractVector, timestep; +function damper_force(relative::Symbol, joint::Rotational{T}, qa::Quaternion, ϕa::AbstractVector, + qb::Quaternion, ϕb::AbstractVector, timestep; rotate::Bool=true, unitary::Bool=false) where T diff --git a/src/joints/rotational/impulses.jl b/src/joints/rotational/impulses.jl index 1f47c8491..4982c211d 100644 --- a/src/joints/rotational/impulses.jl +++ b/src/joints/rotational/impulses.jl @@ -8,8 +8,8 @@ ################################################################################ function impulse_transform_jacobian(relative::Symbol, jacobian::Symbol, joint::Rotational{T,Nλ}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, p; attjac=true) where {T,Nλ} + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, p; attjac=true) where {T,Nλ} if relative == :parent if jacobian == :parent diff --git a/src/joints/rotational/input.jl b/src/joints/rotational/input.jl index c4a1e524b..146c8df84 100644 --- a/src/joints/rotational/input.jl +++ b/src/joints/rotational/input.jl @@ -21,8 +21,8 @@ end function input_jacobian_control(relative::Symbol, joint::Rotational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion) where T + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion) where T if relative == :parent BFa = szeros(T, 3, 3) @@ -37,8 +37,8 @@ end function input_jacobian_configuration(relative::Symbol, joint::Rotational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion) where T + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion) where T τ = joint.input diff --git a/src/joints/rotational/minimal.jl b/src/joints/rotational/minimal.jl index e9f70dfe2..136219f62 100644 --- a/src/joints/rotational/minimal.jl +++ b/src/joints/rotational/minimal.jl @@ -2,8 +2,8 @@ # Displacements ################################################################################ function displacement(joint::Rotational, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; vmat=true) q = inv(joint.axis_offset) * inv(qa) * qb @@ -11,8 +11,8 @@ function displacement(joint::Rotational, end function displacement_jacobian_configuration(relative::Symbol, joint::Rotational, - xa::AbstractVector{T}, qa::UnitQuaternion, - xb::AbstractVector{T}, qb::UnitQuaternion; + xa::AbstractVector{T}, qa::Quaternion, + xb::AbstractVector{T}, qb::Quaternion; attjac::Bool=true, vmat=true) where T X = szeros(T, 3, 3) if relative == :parent @@ -30,15 +30,15 @@ end # Coordinates ################################################################################ function minimal_coordinates(joint::Rotational, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion) + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion) return nullspace_mask(joint) * rotation_vector(displacement(joint, xa, qa, xb, qb, vmat=false)) end function minimal_coordinates_jacobian_configuration(relative::Symbol, joint::Rotational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; attjac::Bool=true) where T A = nullspace_mask(joint) @@ -71,8 +71,8 @@ end # Velocities ################################################################################ function minimal_velocities(joint::Rotational, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ϕb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ϕb::AbstractVector, timestep) axis_offset = joint.axis_offset @@ -91,8 +91,8 @@ function minimal_velocities(joint::Rotational, end function minimal_velocities_jacobian_configuration(relative::Symbol, joint::Rotational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ϕb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ϕb::AbstractVector, timestep) where T axis_offset = joint.axis_offset @@ -124,8 +124,8 @@ function minimal_velocities_jacobian_configuration(relative::Symbol, joint::Rota end function minimal_velocities_jacobian_velocity(relative::Symbol, joint::Rotational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ϕb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ϕb::AbstractVector, timestep) where T axis_offset = joint.axis_offset diff --git a/src/joints/rotational/springs.jl b/src/joints/rotational/springs.jl index 76378bb28..c5c82dac2 100644 --- a/src/joints/rotational/springs.jl +++ b/src/joints/rotational/springs.jl @@ -3,8 +3,8 @@ ################################################################################ function spring_force(relative::Symbol, joint::Rotational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; rotate::Bool=true, unitary::Bool=false) where T @@ -30,8 +30,8 @@ function spring_impulses(relative::Symbol, joint::Rotational, pbody::Node, cbody end function spring_impulses(relative::Symbol, joint::Rotational, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, timestep; unitary::Bool=false) timestep * spring_force(relative, joint, xa, qa, xb, qb; unitary=unitary) @@ -44,8 +44,8 @@ spring_impulses(relative::Symbol, joint::Rotational{T,3}, pbody::Node, cbody::No ################################################################################ function spring_jacobian_configuration(relative::Symbol, jacobian::Symbol, joint::Rotational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, timestep::T; rotate::Bool=true, unitary::Bool=false, diff --git a/src/joints/translational/dampers.jl b/src/joints/translational/dampers.jl index f4562af09..14e56c178 100644 --- a/src/joints/translational/dampers.jl +++ b/src/joints/translational/dampers.jl @@ -3,8 +3,8 @@ ################################################################################ function damper_force(joint::Translational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ωa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ωb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ωa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ωb::AbstractVector, timestep; unitary::Bool=false) where T @@ -16,8 +16,8 @@ function damper_force(joint::Translational{T}, end function damper_force(relative::Symbol, joint::Translational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ωa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ωb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ωa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ωb::AbstractVector, timestep; unitary::Bool=false) where T @@ -42,8 +42,8 @@ damper_impulses(relative::Symbol, joint::Translational{T,3}, pbody::Node, cbody: function damper_force_jacobian_configuration(jacobian_relative::Symbol, joint::Translational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ωa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ωb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ωa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ωb::AbstractVector, timestep; unitary::Bool=false) where T @@ -56,8 +56,8 @@ end function damper_force_jacobian_velocity(jacobian_relative::Symbol, joint::Translational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ωa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ωb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ωa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ωb::AbstractVector, timestep; unitary::Bool=false) where T @@ -70,8 +70,8 @@ end function damper_jacobian_configuration(relative::Symbol, jacobian_relative::Symbol, joint::Translational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ωa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ωb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ωa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ωb::AbstractVector, timestep::T; unitary::Bool=false) where T @@ -99,8 +99,8 @@ end function damper_jacobian_velocity(relative::Symbol, jacobian_relative::Symbol, joint::Translational{T}, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ωa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ωb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ωa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ωb::AbstractVector, timestep::T; unitary::Bool=false) where T diff --git a/src/joints/translational/impulses.jl b/src/joints/translational/impulses.jl index afdb0bb27..340fc570b 100644 --- a/src/joints/translational/impulses.jl +++ b/src/joints/translational/impulses.jl @@ -8,8 +8,8 @@ ################################################################################ function impulse_transform_jacobian(relative::Symbol, jacobian::Symbol, joint::Translational{T,Nλ}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, p; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, p; attjac=true) where {T,Nλ} Z3 = szeros(T,3,3) diff --git a/src/joints/translational/input.jl b/src/joints/translational/input.jl index 1851071db..518e5e64d 100644 --- a/src/joints/translational/input.jl +++ b/src/joints/translational/input.jl @@ -32,8 +32,8 @@ end function input_jacobian_control(relative::Symbol, joint::Translational, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion) where T + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion) where T Ta = impulse_transform(relative, joint, xa, qa, xb, qb) X = Ta[1:3,1:3] @@ -43,8 +43,8 @@ end function input_jacobian_configuration(relative::Symbol, joint::Translational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion) where T + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion) where T # d[Faw;2τaa]/d[xa,qa] ∇aa = impulse_transform_jacobian(:parent, relative, joint, xa, qa, xb, qb, joint.input) diff --git a/src/joints/translational/minimal.jl b/src/joints/translational/minimal.jl index 86d87e980..aa7fde82d 100644 --- a/src/joints/translational/minimal.jl +++ b/src/joints/translational/minimal.jl @@ -2,8 +2,8 @@ # Displacements ################################################################################ function displacement(joint::Translational, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; rotate::Bool = true) vertices = joint.vertices @@ -12,8 +12,8 @@ function displacement(joint::Translational, end function displacement_jacobian_configuration(relative::Symbol, joint::Translational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; attjac=true) where T vertices = joint.vertices @@ -36,12 +36,12 @@ end ################################################################################ # Coordinates ################################################################################ -function minimal_coordinates(joint::Translational, xa::AbstractVector, qa::UnitQuaternion, xb::AbstractVector, qb::UnitQuaternion) +function minimal_coordinates(joint::Translational, xa::AbstractVector, qa::Quaternion, xb::AbstractVector, qb::Quaternion) return nullspace_mask(joint) * displacement(joint, xa, qa, xb, qb) end function minimal_coordinates_jacobian_configuration(relative::Symbol, joint::Translational, - xa::AbstractVector, qa::UnitQuaternion, xb::AbstractVector, qb::UnitQuaternion; attjac::Bool=true) + xa::AbstractVector, qa::Quaternion, xb::AbstractVector, qb::Quaternion; attjac::Bool=true) X, Q = displacement_jacobian_configuration(relative, joint, xa, qa, xb, qb, attjac=attjac) return nullspace_mask(joint) * [X Q] end @@ -73,8 +73,8 @@ end # Velocities ################################################################################ function minimal_velocities(joint::Translational, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ϕb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ϕb::AbstractVector, timestep) A = nullspace_mask(joint) @@ -95,8 +95,8 @@ function minimal_velocities(joint::Translational, end function minimal_velocities_jacobian_configuration(relative::Symbol, joint::Translational, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ϕb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ϕb::AbstractVector, timestep) A = nullspace_mask(joint) @@ -139,8 +139,8 @@ function minimal_velocities_jacobian_configuration(relative::Symbol, joint::Tran end function minimal_velocities_jacobian_velocity(relative::Symbol, joint::Translational, - xa::AbstractVector, va::AbstractVector, qa::UnitQuaternion, ϕa::AbstractVector, - xb::AbstractVector, vb::AbstractVector, qb::UnitQuaternion, ϕb::AbstractVector, + xa::AbstractVector, va::AbstractVector, qa::Quaternion, ϕa::AbstractVector, + xb::AbstractVector, vb::AbstractVector, qb::Quaternion, ϕb::AbstractVector, timestep) A = nullspace_mask(joint) diff --git a/src/joints/translational/springs.jl b/src/joints/translational/springs.jl index 7e3169d5f..32f78ac2d 100644 --- a/src/joints/translational/springs.jl +++ b/src/joints/translational/springs.jl @@ -3,8 +3,8 @@ ################################################################################ function spring_force(relative::Symbol, joint::Translational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; unitary::Bool=false) where T spring = unitary ? 1.0 : joint.spring @@ -22,8 +22,8 @@ function spring_impulses(relative::Symbol, joint::Translational, pbody::Node, cb end function spring_impulses(relative::Symbol, joint::Translational, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, timestep; unitary::Bool=false) timestep * impulse_transform(relative, joint, xa, qa, xb, qb) * spring_force(relative, joint, xa, qa, xb, qb; unitary=unitary)[SVector{3,Int}(1,2,3)] end @@ -36,8 +36,8 @@ spring_impulses(relative::Symbol, joint::Translational{T,3}, pbody::Node, cbody: function spring_force_jacobian_configuration(jacobian_relative::Symbol, joint::Translational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion; + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion; unitary::Bool=false, attjac=true) where T spring = unitary ? 1.0 : joint.spring @@ -46,8 +46,8 @@ end function spring_jacobian_configuration(relative::Symbol, jacobian_relative::Symbol, joint::Translational{T}, - xa::AbstractVector, qa::UnitQuaternion, - xb::AbstractVector, qb::UnitQuaternion, + xa::AbstractVector, qa::Quaternion, + xb::AbstractVector, qb::Quaternion, timestep::T; unitary::Bool=false, attjac=true) where T diff --git a/src/mechanism/data.jl b/src/mechanism/data.jl index d7a58fcd3..74eaf377d 100644 --- a/src/mechanism/data.jl +++ b/src/mechanism/data.jl @@ -139,7 +139,7 @@ function set_data!(body::Body, data::AbstractVector, timestep) v15 = data[SUnitRange(8,10)] ϕ15 = data[SUnitRange(11,13)] x2 = data[SUnitRange(14,16)] - q2 = UnitQuaternion(data[17:20]..., false) + q2 = Quaternion(data[17:20]...) x1 = next_position(x2, -v15, timestep) q1 = next_orientation(q2, -ϕ15, timestep) diff --git a/src/mechanism/set.jl b/src/mechanism/set.jl index 9df3f69dd..6b39a5578 100644 --- a/src/mechanism/set.jl +++ b/src/mechanism/set.jl @@ -11,7 +11,7 @@ function set_maximal_state!(mechanism::Mechanism, z::AbstractVector) off = 0 for body in mechanism.bodies x2, v15, q2, ϕ15 = unpack_data(z[off+1:end]); off += 13 - q2 = UnitQuaternion(q2..., false) + q2 = Quaternion(q2...) body.state.v15 = v15 body.state.ϕ15 = ϕ15 body.state.x2 = x2 diff --git a/src/mechanism/state.jl b/src/mechanism/state.jl index adfb40f5f..17fd474c2 100644 --- a/src/mechanism/state.jl +++ b/src/mechanism/state.jl @@ -55,13 +55,13 @@ function unpack_maximal_state(z::AbstractVector, i::Int) zi = z[(i-1) * 13 .+ (1:13)] x2 = zi[SUnitRange(1,3)] v15 = zi[SUnitRange(4,6)] - q2 = UnitQuaternion(zi[7:10]..., false) + q2 = Quaternion(zi[7:10]...) ϕ15 = zi[SUnitRange(11,13)] return x2, v15, q2, ϕ15 end function pack_maximal_state!(z::AbstractVector, - x2::AbstractVector, v15::AbstractVector, q2::UnitQuaternion, ϕ15::AbstractVector, + x2::AbstractVector, v15::AbstractVector, q2::Quaternion, ϕ15::AbstractVector, i::Int) z[(i-1) * 13 .+ (1:3)] = x2 diff --git a/src/mechanism/urdf.jl b/src/mechanism/urdf.jl index db08bf542..04d1a7d21 100644 --- a/src/mechanism/urdf.jl +++ b/src/mechanism/urdf.jl @@ -47,11 +47,11 @@ end function parse_pose(xpose, T) if xpose === nothing - x, q = zeros(T, 3), one(UnitQuaternion{T}) + x, q = zeros(T, 3), one(Quaternion{T}) else x = parse_vector(xpose, "xyz", T, default = "0 0 0") rpy = parse_vector(xpose, "rpy", T, default = "0 0 0") - q = UnitQuaternion(RotZYX(rpy[3], rpy[2], rpy[1])) + q = RotZ(rpy[3])*RotY(rpy[2])*RotX(rpy[1]) # Quaternion(RotZYX(rpy[3], rpy[2], rpy[1])) end return x, q @@ -60,7 +60,7 @@ end function parse_inertia(xinertial, T) if xinertial === nothing x = zeros(T, 3) - q = one(UnitQuaternion{T}) + q = one(Quaternion{T}) m = zero(T) J = zeros(T, 3, 3) else @@ -212,10 +212,10 @@ end # TODO: fix axis function joint_selector(joint_type, pbody, cbody, T; - axis = SA{T}[1;0;0], parent_vertex = szeros(T,3), child_vertex = szeros(T,3), axis_offset = one(UnitQuaternion{T}), name = Symbol("joint_" * randstring(4))) + axis = SA{T}[1;0;0], parent_vertex = szeros(T,3), child_vertex = szeros(T,3), axis_offset = one(Quaternion{T}), name = Symbol("joint_" * randstring(4))) # TODO @warn "this is not great" - axis = inv(axis_offset) * axis + axis = vector_rotate(axis, axis_offset) # inv(axis_offset) * axis # TODO limits for revolute joint? if joint_type == "revolute" || joint_type == "continuous" @@ -419,7 +419,7 @@ function set_parsed_values!(mechanism::Mechanism{T}, loopjoints) where T system = mechanism.system timestep= mechanism.timestep xjointlist = Dict{Int64,SVector{3,T}}() # stores id, x in world frame - qjointlist = Dict{Int64,UnitQuaternion{T}}() # stores id, q in world frame + qjointlist = Dict{Int64,Quaternion{T}}() # stores id, q in world frame for id in root_to_leaves_ordering(mechanism, exclude_origin=true, exclude_loop_joints=true) node = get_node(mechanism, id) @@ -450,10 +450,10 @@ function set_parsed_values!(mechanism::Mechanism{T}, loopjoints) where T # Parent joint if pnode.id == 0 # parent body is origin # x_pjoint = SA{T}[0; 0; 0] - # q_pjoint = one(UnitQuaternion{T}) + # q_pjoint = one(Quaternion{T}) # # axis_pjoint = SA{T}[1; 0; 0] xparentjoint = SA{T}[0; 0; 0] - qparentjoint = one(UnitQuaternion{T}) + qparentjoint = one(Quaternion{T}) else pjoint = get_node(mechanism, get_parent_id(mechanism, pnode.id, loopjoints)) # x_pjoint = pjoint.translational.vertices[1] # stored in parent_vertex @@ -512,10 +512,10 @@ function set_parsed_values!(mechanism::Mechanism{T}, loopjoints) where T parentpbody = mechanism.origin xparentpbody = SA{T}[0; 0; 0] - qparentpbody = one(UnitQuaternion{T}) + qparentpbody = one(Quaternion{T}) xparentjoint1 = SA{T}[0; 0; 0] - qparentjoint1 = one(UnitQuaternion{T}) + qparentjoint1 = one(Quaternion{T}) else parentpbody = get_body(mechanism, parent_id1) diff --git a/src/orientation/axis_angle.jl b/src/orientation/axis_angle.jl index 1a9e14e6e..f5f59d908 100644 --- a/src/orientation/axis_angle.jl +++ b/src/orientation/axis_angle.jl @@ -3,9 +3,9 @@ function axis_angle_to_quaternion(x) θ = norm(x) if θ > 0.0 r = x ./ θ - q = UnitQuaternion(cos(0.5 * θ), sin(0.5 * θ) * r, false) + q = Quaternion(cos(0.5 * θ), sin(0.5 * θ) * r) else - q = UnitQuaternion(1.0, 0.0, 0.0, 0.0, false) + q = Quaternion(1.0, 0.0, 0.0, 0.0) end return q end diff --git a/src/orientation/mapping.jl b/src/orientation/mapping.jl index 33adf29a5..197c05eaf 100644 --- a/src/orientation/mapping.jl +++ b/src/orientation/mapping.jl @@ -1,5 +1,5 @@ function quaternion_map(ω, timestep) - return UnitQuaternion(sqrt(4 / timestep^2 - dot(ω, ω)), ω, false) + return Quaternion(sqrt(4 / timestep^2 - dot(ω, ω)), ω) end function quaternion_map_jacobian(ω::SVector{3}, timestep) @@ -8,7 +8,7 @@ function quaternion_map_jacobian(ω::SVector{3}, timestep) end function cayley(ω) - UnitQuaternion(1.0 / sqrt(1.0 + ω' * ω) * [1.0; ω], false) + Quaternion(1.0 / sqrt(1.0 + ω' * ω) * [1.0; ω]) end function cayley_jacobian(ω) diff --git a/src/orientation/mrp.jl b/src/orientation/mrp.jl index 60af259a2..72dc938a4 100644 --- a/src/orientation/mrp.jl +++ b/src/orientation/mrp.jl @@ -1,4 +1,4 @@ -function mrp(q::UnitQuaternion) +function mrp(q::Quaternion) q̄ = vector(q) return q̄[2:4] ./ (q̄[1] + 1.0) end @@ -77,4 +77,4 @@ function drotation_vectordq(q::AbstractVector) end end -drotation_vectordq(q::UnitQuaternion) = drotation_vectordq(vector(q)) +drotation_vectordq(q::Quaternion) = drotation_vectordq(vector(q)) diff --git a/src/orientation/quaternion.jl b/src/orientation/quaternion.jl index 27d1db91e..9f60af883 100644 --- a/src/orientation/quaternion.jl +++ b/src/orientation/quaternion.jl @@ -1,93 +1,130 @@ -Rotations.UnitQuaternion(w::T, v::StaticVector{3,T}, normalize::Bool = true) where T = UnitQuaternion{T}(w, v[1], v[2], v[3], normalize) -Rotations.UnitQuaternion(w::T, v::Vector{T}, normalize::Bool = true) where T = (@assert length(v)==3; UnitQuaternion{T}(w, v[1], v[2], v[3], normalize)) -Rotations.UnitQuaternion(v::StaticVector{3,T}) where T = pure_quaternion(v) -# Rotations.UnitQuaternion(v::Vector) = (@assert length(v)==3; pure_quaternion(v)) +MeshCat.LinearMap(q::Quaternion) = MeshCat.LinearMap(rotation_matrix(q)) +MeshCat.js_quaternion(q::Quaternion) = [q.v1, q.v2, q.v3, q.s] -imag(q::UnitQuaternion) = Rotations.vector(q) +RotX(θ) = Quaternion(cos(θ/2), sin(θ/2), 0, 0) +RotY(θ) = Quaternion(cos(θ/2), 0, sin(θ/2), 0) +RotZ(θ) = Quaternion(cos(θ/2), 0, 0, sin(θ/2)) -Lmat(q) = lmult(q) -Lᵀmat(q) = lmult(q)' -Rmat(q) = rmult(q) -Rᵀmat(q) = rmult(q)' +quateltype(x) = eltype(x) # TODO not super elegant +quateltype(::Quaternion{T}) where T = T -# Remove once added to Rotations.jl -function Base.:/(q::UnitQuaternion, w::Real) - return UnitQuaternion(q.w/w, q.x/w, q.y/w, q.z/w, false) +vector(q::Quaternion) = SA[q.s, q.v1, q.v2, q.v3] +vector(q::AbstractVector) = q + +function Lmat(q::Quaternion) + SA[ + q.s -q.v1 -q.v2 -q.v3; + q.v1 q.s -q.v3 q.v2; + q.v2 q.v3 q.s -q.v1; + q.v3 -q.v2 q.v1 q.s; + ] end -Tmat(::Type{T}=Float64) where T = tmat(T) -Vmat(::Type{T}=Float64) where T = vmat(T) -Vᵀmat(::Type{T}=Float64) where T = hmat(T) -Vmat(q::UnitQuaternion) = imag(q) +function Rmat(q::Quaternion) + SA[ + q.s -q.v1 -q.v2 -q.v3; + q.v1 q.s q.v3 -q.v2; + q.v2 -q.v3 q.s q.v1; + q.v3 q.v2 -q.v1 q.s; + ] +end -vector(q::UnitQuaternion) = SA[q.w, q.x, q.y, q.z] -vector(q::AbstractVector) = q +Lᵀmat(q) = Lmat(q)' +Rᵀmat(q) = Rmat(q)' + +function Tmat(::Type{T}=Float64) where T + SA{T}[ + 1 0 0 0; + 0 -1 0 0; + 0 0 -1 0; + 0 0 0 -1; + ] +end -function VLmat(q::UnitQuaternion) +function Vmat(::Type{T}=Float64) where T + SA{T}[ + 0 1 0 0; + 0 0 1 0; + 0 0 0 1; + ] +end + +function Vᵀmat(::Type{T}=Float64) where T + SA{T}[ + 0 0 0; + 1 0 0; + 0 1 0; + 0 0 1; + ] +end + +Vmat(q::Quaternion) = SA[q.v1, q.v2, q.v3] + +function VLmat(q::Quaternion) SA[ - q.x q.w -q.z q.y; - q.y q.z q.w -q.x; - q.z -q.y q.x q.w; + q.v1 q.s -q.v3 q.v2; + q.v2 q.v3 q.s -q.v1; + q.v3 -q.v2 q.v1 q.s; ] end -function VLᵀmat(q::UnitQuaternion) +function VLᵀmat(q::Quaternion) SA[ - -q.x q.w q.z -q.y; - -q.y -q.z q.w q.x; - -q.z q.y -q.x q.w; + -q.v1 q.s q.v3 -q.v2; + -q.v2 -q.v3 q.s q.v1; + -q.v3 q.v2 -q.v1 q.s; ] end -function VRmat(q::UnitQuaternion) +function VRmat(q::Quaternion) SA[ - q.x q.w q.z -q.y; - q.y -q.z q.w q.x; - q.z q.y -q.x q.w; + q.v1 q.s q.v3 -q.v2; + q.v2 -q.v3 q.s q.v1; + q.v3 q.v2 -q.v1 q.s; ] end -function VRᵀmat(q::UnitQuaternion) +function VRᵀmat(q::Quaternion) SA[ - -q.x q.w -q.z q.y; - -q.y q.z q.w -q.x; - -q.z -q.y q.x q.w; + -q.v1 q.s -q.v3 q.v2; + -q.v2 q.v3 q.s -q.v1; + -q.v3 -q.v2 q.v1 q.s; ] end -function LVᵀmat(q::UnitQuaternion) +function LVᵀmat(q::Quaternion) SA[ - -q.x -q.y -q.z; - q.w -q.z q.y; - q.z q.w -q.x; - -q.y q.x q.w; + -q.v1 -q.v2 -q.v3; + q.s -q.v3 q.v2; + q.v3 q.s -q.v1; + -q.v2 q.v1 q.s; ] end -function LᵀVᵀmat(q::UnitQuaternion) +function LᵀVᵀmat(q::Quaternion) SA[ - q.x q.y q.z; - q.w q.z -q.y; - -q.z q.w q.x; - q.y -q.x q.w; + q.v1 q.v2 q.v3; + q.s q.v3 -q.v2; + -q.v3 q.s q.v1; + q.v2 -q.v1 q.s; ] end -function RVᵀmat(q::UnitQuaternion) +function RVᵀmat(q::Quaternion) SA[ - -q.x -q.y -q.z; - q.w q.z -q.y; - -q.z q.w q.x; - q.y -q.x q.w; + -q.v1 -q.v2 -q.v3; + q.s q.v3 -q.v2; + -q.v3 q.s q.v1; + q.v2 -q.v1 q.s; ] end -function RᵀVᵀmat(q::UnitQuaternion) +function RᵀVᵀmat(q::Quaternion) SA[ - q.x q.y q.z; - q.w -q.z q.y; - q.z q.w -q.x; - -q.y q.x q.w; + q.v1 q.v2 q.v3; + q.s -q.v3 q.v2; + q.v3 q.s -q.v1; + -q.v2 q.v1 q.s; ] end @@ -171,6 +208,14 @@ function ∂Lmat∂q(p::AbstractVector) # 𝞉(Lmat(q)*p)/∂q ] end +function skew(p) + SA[ + 0 -p[3] p[2]; + p[3] 0 -p[1]; + -p[2] p[1] 0; + ] +end + function ∂skew∂p(λ) # 𝞉(skew(p)*λ)/∂p skew(-λ) end \ No newline at end of file diff --git a/src/orientation/rotate.jl b/src/orientation/rotate.jl index 0c15edc67..7c9ce8959 100644 --- a/src/orientation/rotate.jl +++ b/src/orientation/rotate.jl @@ -1,14 +1,14 @@ # rotate quaternion -quaternion_rotate(q1::UnitQuaternion,q2::UnitQuaternion) = q2 * q1 / q2 +quaternion_rotate(q1::Quaternion,q2::Quaternion) = q2 * q1 / q2 # rotate vector -vector_rotate(v::Vector,q::UnitQuaternion) = imag(quaternion_rotate(pure_quaternion(v), q)) -vector_rotate(v::StaticVector,q::UnitQuaternion) = q*v -∂vector_rotate∂p(p::AbstractVector, q::UnitQuaternion) = VRᵀmat(q) * LVᵀmat(q) -∂vector_rotate∂q(p::AbstractVector, q::UnitQuaternion) = VLmat(q) * Lmat(UnitQuaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(UnitQuaternion(p)) +vector_rotate(v::AbstractVector,q::Quaternion) = Vmat(quaternion_rotate(Quaternion(v), q)) +# vector_rotate(v::StaticVector,q::Quaternion) = q*v +∂vector_rotate∂p(p::AbstractVector, q::Quaternion) = VRᵀmat(q) * LVᵀmat(q) +∂vector_rotate∂q(p::AbstractVector, q::Quaternion) = VLmat(q) * Lmat(Quaternion(p)) * Tmat() + VRᵀmat(q) * Rmat(Quaternion(p)) # rotation matrix -rotation_matrix(q::UnitQuaternion) = VRᵀmat(q) * LVᵀmat(q) -∂rotation_matrix∂q(q::UnitQuaternion, p::AbstractVector) = ∂VRᵀmat∂q(LVᵀmat(q) * p) + VRᵀmat(q) * ∂LVᵀmat∂q(p) -∂rotation_matrix_inv∂q(q::UnitQuaternion, p::AbstractVector) = ∂rotation_matrix∂q(inv(q), p) * Tmat() +rotation_matrix(q::Quaternion) = VRᵀmat(q) * LVᵀmat(q) +∂rotation_matrix∂q(q::Quaternion, p::AbstractVector) = ∂VRᵀmat∂q(LVᵀmat(q) * p) + VRᵀmat(q) * ∂LVᵀmat∂q(p) +∂rotation_matrix_inv∂q(q::Quaternion, p::AbstractVector) = ∂rotation_matrix∂q(inv(q), p) * Tmat() diff --git a/src/simulation/storage.jl b/src/simulation/storage.jl index 9b953a5ee..0e78c0cee 100644 --- a/src/simulation/storage.jl +++ b/src/simulation/storage.jl @@ -4,7 +4,7 @@ contains maximal-representation trajectories x: position - q: orientation (UnitQuaternion) + q: orientation (Quaternion) v: linear velocity (midpoint) ω: angular velocity (midpoint) px: linear momentum @@ -14,7 +14,7 @@ """ struct Storage{T,N} x::Vector{Vector{SVector{3,T}}} - q::Vector{Vector{UnitQuaternion{T}}} + q::Vector{Vector{Quaternion{T}}} v::Vector{Vector{SVector{3,T}}} ω::Vector{Vector{SVector{3,T}}} px::Vector{Vector{SVector{3,T}}} @@ -24,7 +24,7 @@ struct Storage{T,N} function Storage{T}(steps, nbodies) where T x = [[szeros(T, 3) for i = steps] for j = 1:nbodies] - q = [[one(UnitQuaternion{T}) for i = steps] for j = 1:nbodies] + q = [[one(Quaternion{T}) for i = steps] for j = 1:nbodies] v = [[szeros(T, 3) for i = steps] for j = 1:nbodies] ω = [[szeros(T, 3) for i = steps] for j = 1:nbodies] px = [[szeros(T, 3) for i = steps] for j = 1:nbodies] @@ -74,7 +74,7 @@ function generate_storage(mechanism::Mechanism, z) for i = 1:M storage.x[i][t] = z[t][off .+ (1:3)] storage.v[i][t] = z[t][off .+ (4:6)] - storage.q[i][t] = UnitQuaternion(z[t][off .+ (7:10)]..., false) + storage.q[i][t] = Quaternion(z[t][off .+ (7:10)]...) storage.ω[i][t] = z[t][off .+ (11:13)] off += 13 end diff --git a/src/visuals/set.jl b/src/visuals/set.jl index 2915c6ca4..d1710cf12 100644 --- a/src/visuals/set.jl +++ b/src/visuals/set.jl @@ -101,7 +101,7 @@ function set_camera!(vis::Visualizer; setprop!(camvis, "zoom", zoom) (cam_pos != nothing) && MeshCat.settransform!(camvis, MeshCat.compose( - MeshCat.LinearMap(Rotations.RotX(-1/2 * pi)), + MeshCat.LinearMap(Dojo.RotX(-1/2 * pi)), MeshCat.Translation(cam_pos...), )) return nothing diff --git a/src/visuals/visualizer.jl b/src/visuals/visualizer.jl index 576e03b7e..12353dc66 100644 --- a/src/visuals/visualizer.jl +++ b/src/visuals/visualizer.jl @@ -76,7 +76,7 @@ function visualize(mechanism::Mechanism, storage::Storage{T,N}; vis::Visualizer= (radius == 0.0) && (radius = 0.01) contact_shape = Sphere(radius, position_offset=contact.model.contact_point, - axis_offset=one(UnitQuaternion), + axis_offset=one(Quaternion), color=RGBA(1.0, 0.0, 0.0, 0.5)) visshape = convert_shape(contact_shape) subvisshape = nothing @@ -97,7 +97,7 @@ function visualize(mechanism::Mechanism, storage::Storage{T,N}; vis::Visualizer= visshape = convert_shape(shape) if visshape !== nothing subvisshape = vis[name][:bodies][Symbol(:origin, "_id")] - shapetransform = transform(szeros(T,3), one(UnitQuaternion{T}), shape) + shapetransform = transform(szeros(T,3), one(Quaternion{T}), shape) settransform!(subvisshape, shapetransform) end @@ -170,7 +170,7 @@ function build_robot(mechanism::Mechanism; vis::Visualizer=Visualizer(), (radius == 0.0) && (radius = 0.01) contact_shape = Sphere(radius, position_offset=(contact.model.contact_point), - axis_offset=one(UnitQuaternion), color=RGBA(1.0, 0.0, 0.0, 0.5)) + axis_offset=one(Quaternion), color=RGBA(1.0, 0.0, 0.0, 0.5)) visshape = convert_shape(contact_shape) subvisshape = nothing if visshape !== nothing @@ -254,7 +254,7 @@ function set_robot(vis::Visualizer, mechanism::Mechanism, z::Vector{T}; (radius == 0.0) && (radius = 0.01) contact_shape = Sphere(radius, position_offset=(contact.model.contact_point), - axis_offset=one(UnitQuaternion), color=RGBA(1.0, 0.0, 0.0, 0.5)) + axis_offset=one(Quaternion), color=RGBA(1.0, 0.0, 0.0, 0.5)) visshape = convert_shape(contact_shape) subvisshape = nothing showshape = false @@ -274,7 +274,7 @@ function set_robot(vis::Visualizer, mechanism::Mechanism, z::Vector{T}; visshape = convert_shape(shape) if visshape !== nothing subvisshape = vis[name][:bodies][Symbol(:origin, "_id")] - shapetransform = transform(szeros(T,3), one(UnitQuaternion{T}), shape) + shapetransform = transform(szeros(T,3), one(Quaternion{T}), shape) settransform!(subvisshape, shapetransform) end return vis diff --git a/test/Project.toml b/test/Project.toml index 2694b2c60..6543d2198 100644 --- a/test/Project.toml +++ b/test/Project.toml @@ -3,6 +3,7 @@ BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" +Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" SafeTestsets = "1bc83da4-3b8d-516f-aca4-4fe02f6d838f" SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" @@ -13,3 +14,4 @@ BenchmarkTools = "1.2.0" FiniteDiff = "2.8.1" SafeTestsets = "0.0.1" StaticArrays = "0.8.0 - 1.4.1" +Rotations = "1.1" diff --git a/test/energy.jl b/test/energy.jl index da9a58634..f635d4c72 100644 --- a/test/energy.jl +++ b/test/energy.jl @@ -480,7 +480,7 @@ mech = get_mechanism(:snake, v0 = 10.0 * [1, 2, 3] * timestep0 ω0 = 10.0 * [1, 2, 3] * timestep0 -q10 = UnitQuaternion(RotX(0.5*π)) +q10 = Dojo.RotX(0.5*π) initialize!(mech, :snake, base_orientation=q10, @@ -522,7 +522,7 @@ norm((me0 .- me0[1]) ./ mean(me0), Inf) v0 = 10.0 * [1, 2, 3] * timestep0 ω0 = 10.0 * [1, 2, 3] * timestep0 - q10 = UnitQuaternion(RotX(0.5*π)) + q10 = Dojo.RotX(0.5*π) initialize!(mech, :snake, base_orientation=q10, @@ -586,7 +586,7 @@ mech = get_mechanism(:twister, v0 = 10.0 * [1, 2, 3] * timestep0 ω0 = 10.0 * [1, 2, 3] * timestep0 -q10 = UnitQuaternion(RotX(0.5*π)) +q10 = Dojo.RotX(0.5*π) initialize!(mech, :twister, base_orientation=q10, @@ -627,7 +627,7 @@ norm((me0 .- me0[1]) ./ mean(me0), Inf) v0 = 10.0 * [1, 2, 3] * timestep0 ω0 = 10.0 * [1, 2, 3] * timestep0 - q10 = UnitQuaternion(RotX(0.5*π)) + q10 = Dojo.RotX(0.5*π) initialize!(mech, :twister, base_orientation=q10, diff --git a/test/impulse_map.jl b/test/impulse_map.jl index eca570f99..4211e786d 100644 --- a/test/impulse_map.jl +++ b/test/impulse_map.jl @@ -5,12 +5,12 @@ mech = Dojo.get_mechanism(:pendulum) joint0 = mech.joints[1] rot0 = joint0.rotational - rot0.axis_offset = UnitQuaternion(rand(4)...) + rot0.axis_offset = rand(QuatRotation).q xa = rand(3) - qa = UnitQuaternion(rand(4)...) + qa = rand(QuatRotation).q xb = rand(3) - qb = UnitQuaternion(rand(4)...) + qb = rand(QuatRotation).q # displacement_jacobian_configuration X0, Q0 = Dojo.displacement_jacobian_configuration(:parent, rot0, xa, qa, xb, qb, @@ -18,7 +18,7 @@ J0 = [X0 Q0] attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.displacement(rot0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb), + z -> Dojo.displacement(rot0, z[1:3], Quaternion(z[4:7]...), xb, qb), [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -29,7 +29,7 @@ J0 = [X0 Q0] attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.displacement(rot0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false)), + z -> Dojo.displacement(rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)), [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -41,9 +41,9 @@ end tra0 = joint0.translational xa = rand(3) - qa = UnitQuaternion(rand(4)...) + qa = rand(QuatRotation).q xb = rand(3) - qb = UnitQuaternion(rand(4)...) + qb = rand(QuatRotation).q # displacement_jacobian_configuration X0, Q0 = Dojo.displacement_jacobian_configuration(:parent, tra0, xa, qa, xb, qb, @@ -51,7 +51,7 @@ end J0 = [X0 Q0] attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.displacement(tra0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb), + z -> Dojo.displacement(tra0, z[1:3], Quaternion(z[4:7]...), xb, qb), [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -62,7 +62,7 @@ end J0 = [X0 Q0] attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.displacement(tra0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false)), + z -> Dojo.displacement(tra0, xa, qa, z[1:3], Quaternion(z[4:7]...)), [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -76,18 +76,18 @@ end mech = Dojo.get_mechanism(:pendulum) joint0 = mech.joints[1] rot0 = joint0.rotational - rot0.axis_offset = UnitQuaternion(rand(4)...) + rot0.axis_offset = rand(QuatRotation).q xa = rand(3) - qa = UnitQuaternion(rand(4)...) + qa = rand(QuatRotation).q xb = rand(3) - qb = UnitQuaternion(rand(4)...) + qb = rand(QuatRotation).q p0 = rand(3) J0 = Dojo.impulse_transform_jacobian(:parent, :parent, rot0, xa, qa, xb, qb, p0) attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:parent, rot0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb) * p0, + z -> Dojo.impulse_transform(:parent, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -95,7 +95,7 @@ end J0 = Dojo.impulse_transform_jacobian(:parent, :child, rot0, xa, qa, xb, qb, p0) attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:parent, rot0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false)) * p0, + z -> Dojo.impulse_transform(:parent, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -103,7 +103,7 @@ end J0 = Dojo.impulse_transform_jacobian(:child, :parent, rot0, xa, qa, xb, qb, p0) attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:child, rot0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb) * p0, + z -> Dojo.impulse_transform(:child, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -111,7 +111,7 @@ end J0 = Dojo.impulse_transform_jacobian(:child, :child, rot0, xa, qa, xb, qb, p0) attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:child, rot0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false)) * p0, + z -> Dojo.impulse_transform(:child, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -123,15 +123,15 @@ end tra0 = joint0.translational xa = rand(3) - qa = UnitQuaternion(rand(4)...) + qa = rand(QuatRotation).q xb = rand(3) - qb = UnitQuaternion(rand(4)...) + qb = rand(QuatRotation).q p0 = rand(3) J0 = Dojo.impulse_transform_jacobian(:parent, :parent, tra0, xa, qa, xb, qb, p0) attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:parent, tra0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb) * p0, + z -> Dojo.impulse_transform(:parent, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -139,7 +139,7 @@ end J0 = Dojo.impulse_transform_jacobian(:parent, :child, tra0, xa, qa, xb, qb, p0) attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:parent, tra0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false)) * p0, + z -> Dojo.impulse_transform(:parent, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -147,7 +147,7 @@ end J0 = Dojo.impulse_transform_jacobian(:child, :parent, tra0, xa, qa, xb, qb, p0) attjac = cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:child, tra0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb) * p0, + z -> Dojo.impulse_transform(:child, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -155,7 +155,7 @@ end J0 = Dojo.impulse_transform_jacobian(:child, :child, tra0, xa, qa, xb, qb, p0) attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_transform(:child, tra0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false)) * p0, + z -> Dojo.impulse_transform(:child, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -171,9 +171,9 @@ end tra0 = joint0.translational xa = rand(3) - qa = UnitQuaternion(rand(4)...) + qa = rand(QuatRotation).q xb = rand(3) - qb = UnitQuaternion(rand(4)...) + qb = rand(QuatRotation).q λ = rand(3) η = rand(0) Dojo.impulse_map(:parent, tra0, xa, qa, xb, qb, η) @@ -192,7 +192,7 @@ end attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:parent, tra0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb, 0) * λ, + z -> Dojo.impulse_map(:parent, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -201,7 +201,7 @@ end J0 = Dojo.impulse_map_jacobian(:parent, :child, tra0, pbody, cbody, λ) attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:parent, tra0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false), 0) * λ, + z -> Dojo.impulse_map(:parent, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -210,7 +210,7 @@ end J0 = Dojo.impulse_map_jacobian(:child, :parent, tra0, pbody, cbody, λ) attjac = cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:child, tra0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb, 0) * λ, + z -> Dojo.impulse_map(:child, tra0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -219,7 +219,7 @@ end J0 = Dojo.impulse_map_jacobian(:child, :child, tra0, pbody, cbody, λ) attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:child, tra0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false), 0) * λ, + z -> Dojo.impulse_map(:child, tra0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -229,13 +229,13 @@ end mech = Dojo.get_mechanism(:pendulum) joint0 = mech.joints[1] rot0 = joint0.rotational - rot0.axis_offset = UnitQuaternion(rand(4)...) + rot0.axis_offset = rand(QuatRotation).q xa = rand(3) - qa = UnitQuaternion(rand(4)...) + qa = rand(QuatRotation).q xb = rand(3) - qb = UnitQuaternion(rand(4)...) + qb = rand(QuatRotation).q λ = rand(2) η = rand(0) Dojo.impulse_map(:parent, rot0, xa, qa, xb, qb, η) @@ -253,7 +253,7 @@ end J0 = Dojo.impulse_map_jacobian(:parent, :parent, rot0, pbody, cbody, λ) attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:parent, rot0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb, 0) * λ, + z -> Dojo.impulse_map(:parent, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -262,7 +262,7 @@ end J0 = Dojo.impulse_map_jacobian(:parent, :child, rot0, pbody, cbody, λ) attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:parent, rot0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false), 0) * λ, + z -> Dojo.impulse_map(:parent, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -271,7 +271,7 @@ end J0 = Dojo.impulse_map_jacobian(:child, :parent, rot0, pbody, cbody, λ) attjac = cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:child, rot0, z[1:3], UnitQuaternion(z[4:7]..., false), xb, qb, 0) * λ, + z -> Dojo.impulse_map(:child, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb, 0) * λ, [xa; Dojo.vector(qa)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 @@ -280,7 +280,7 @@ end J0 = Dojo.impulse_map_jacobian(:child, :child, rot0, pbody, cbody, λ) attjac = cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) J1 = FiniteDiff.finite_difference_jacobian( - z -> Dojo.impulse_map(:child, rot0, xa, qa, z[1:3], UnitQuaternion(z[4:7]..., false), 0) * λ, + z -> Dojo.impulse_map(:child, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...), 0) * λ, [xb; Dojo.vector(qb)] ) * attjac @test norm(J0 - J1, Inf) < 1.0e-7 diff --git a/test/integrator.jl b/test/integrator.jl index b66d26180..5aac8eb23 100644 --- a/test/integrator.jl +++ b/test/integrator.jl @@ -2,7 +2,7 @@ Random.seed!(100) x0 = srand(3) v0 = srand(3) - q0 = UnitQuaternion(rand(4)...) + q0 = rand(QuatRotation).q ϕ0 = srand(3) timestep0 = 0.01 x1 = Dojo.next_position(x0, v0, timestep0) @@ -20,7 +20,7 @@ @test norm(FD∂i∂x - Dojo.linear_integrator_jacobian_position(), Inf) < 1.0e-6 # ∇q FD∂i∂q = FiniteDiff.finite_difference_jacobian(q0 -> - Dojo.vector(Dojo.next_orientation(UnitQuaternion(q0..., false), ϕ0, timestep0)), + Dojo.vector(Dojo.next_orientation(Quaternion(q0...), ϕ0, timestep0)), Dojo.vector(q0)) @test norm(FD∂i∂q - Dojo.rotational_integrator_jacobian_orientation(q0, ϕ0, timestep0, attjac=false), Inf) < 1.0e-6 @@ -35,7 +35,7 @@ con0 = [x0; Dojo.vector(q0)] FDcon = FiniteDiff.finite_difference_jacobian(con0 -> [Dojo.next_position(SVector{3}(con0[1:3]), v0, timestep0); - Dojo.vector(Dojo.next_orientation(UnitQuaternion(con0[4:7]..., false), ϕ0, timestep0))], con0) - @test norm(FDcon - Dojo.integrator_jacobian_configuration(q0, ϕ0, timestep0, attjac=false), Inf) < 1.0e-6 - @test norm(FDcon * cat(I(3), Dojo.LVᵀmat(q0), dims=(1,2)) - Dojo.integrator_jacobian_configuration(q0, ϕ0, timestep0, attjac=true), Inf) < 1.0e-6 + Dojo.vector(Dojo.next_orientation(Quaternion(con0[4:7]...), ϕ0, timestep0))], con0) + @test norm(FDcon - Dojo.integrator_jacobian_configuration(q0, ϕ0, timestep0, attjac=false), Inf) < 1e-8 + @test norm(FDcon * cat(I(3), Dojo.LVᵀmat(q0), dims=(1,2)) - Dojo.integrator_jacobian_configuration(q0, ϕ0, timestep0, attjac=true), Inf) < 1e-8 end diff --git a/test/minimal.jl b/test/minimal.jl index 71947d1bf..a35e9db10 100644 --- a/test/minimal.jl +++ b/test/minimal.jl @@ -35,7 +35,7 @@ joint_types = [ x = srand(1) Δx = Dojo.zerodimstaticadjoint(Dojo.nullspace_mask(tra2)) * x - Δq = UnitQuaternion(rand(4)...) + Δq = rand(QuatRotation).q Dojo.set_maximal_configurations!(pbody, cbody; parent_vertex=tra2.vertices[1], child_vertex=tra2.vertices[2], @@ -223,7 +223,7 @@ end timestep = mech.timestep for joint in mech.joints - joint.rotational.axis_offset = UnitQuaternion(rand(4)...) + joint.rotational.axis_offset = rand(QuatRotation).q end joint0 = mech.joints[1] tra0 = joint0.translational @@ -283,12 +283,12 @@ end xa = SVector{3}(zp[1:3]) va = SVector{3}(zp[3 .+ (1:3)]) - qa = UnitQuaternion(zp[6 .+ (1:4)]..., false) + qa = Quaternion(zp[6 .+ (1:4)]...) ωa = SVector{3}(zp[10 .+ (1:3)]) xb = SVector{3}(zc[1:3]) vb = SVector{3}(zc[3 .+ (1:3)]) - qb = UnitQuaternion(zc[6 .+ (1:4)]..., false) + qb = Quaternion(zc[6 .+ (1:4)]...) ωb = SVector{3}(zc[10 .+ (1:3)]) Dojo.minimal_velocities(joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep) @@ -296,13 +296,13 @@ end # Jacobians ∇0 = Dojo.minimal_velocities_jacobian_configuration(:parent, joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep) ∇1 = FiniteDiff.finite_difference_jacobian( - xq -> Dojo.minimal_velocities(joint, xq[Dojo.SUnitRange(1,3)], va, UnitQuaternion(xq[4:7]..., false), ωa, xb, vb, qb, ωb, timestep), + xq -> Dojo.minimal_velocities(joint, xq[Dojo.SUnitRange(1,3)], va, Quaternion(xq[4:7]...), ωa, xb, vb, qb, ωb, timestep), [xa; Dojo.vector(qa)]) * cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2)) @test norm(∇0 - ∇1, Inf) < 1.0e-5 ∇0 = Dojo.minimal_velocities_jacobian_configuration(:child, joint, xa, va, qa, ωa, xb, vb, qb, ωb, timestep) ∇1 = FiniteDiff.finite_difference_jacobian( - xq -> Dojo.minimal_velocities(joint, xa, va, qa, ωa, xq[Dojo.SUnitRange(1,3)], vb, UnitQuaternion(xq[4:7]..., false), ωb, timestep), + xq -> Dojo.minimal_velocities(joint, xa, va, qa, ωa, xq[Dojo.SUnitRange(1,3)], vb, Quaternion(xq[4:7]...), ωb, timestep), [xb; Dojo.vector(qb)]) * cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) @test norm(∇0 - ∇1, Inf) < 1.0e-5 @@ -342,25 +342,25 @@ end xa = SVector{3}(zp[1:3]) # va = SVector{3}(zp[3 .+ (1:3)]) - qa = UnitQuaternion(zp[6 .+ (1:4)]..., false) + qa = Quaternion(zp[6 .+ (1:4)]...) # ωa = SVector{3}(zp[10 .+ (1:3)]) xb = SVector{3}(zc[1:3]) # vb = SVector{3}(zc[3 .+ (1:3)]) - qb = UnitQuaternion(zc[6 .+ (1:4)]..., false) + qb = Quaternion(zc[6 .+ (1:4)]...) # ωb = SVector{3}(zc[10 .+ (1:3)]) Dojo.minimal_coordinates(joint, xa, qa, xb, qb) ∇0 = Dojo.minimal_coordinates_jacobian_configuration(:parent, joint, xa, qa, xb, qb) ∇1 = FiniteDiff.finite_difference_jacobian( - xq -> Dojo.minimal_coordinates(joint, xq[1:3], UnitQuaternion(xq[4:7]..., false), xb, qb), + xq -> Dojo.minimal_coordinates(joint, xq[1:3], Quaternion(xq[4:7]...), xb, qb), [xa; Dojo.vector(qa)]) * cat(I(3), Dojo.LVᵀmat(qa), dims=(1,2)) @test norm(∇0 - ∇1, Inf) < 1.0e-6 ∇0 = Dojo.minimal_coordinates_jacobian_configuration(:child, joint, xa, qa, xb, qb) ∇1 = FiniteDiff.finite_difference_jacobian( - xq -> Dojo.minimal_coordinates(joint, xa, qa, xq[1:3], UnitQuaternion(xq[4:7]..., false)), + xq -> Dojo.minimal_coordinates(joint, xa, qa, xq[1:3], Quaternion(xq[4:7]...)), [xb; Dojo.vector(qb)]) * cat(I(3), Dojo.LVᵀmat(qb), dims=(1,2)) @test norm(∇0 - ∇1, Inf) < 1.0e-6 end diff --git a/test/momentum.jl b/test/momentum.jl index 4f08adfdf..345c367af 100644 --- a/test/momentum.jl +++ b/test/momentum.jl @@ -243,7 +243,7 @@ mech = get_mechanism(:snake, v0 = 100.0 * [1, 2, 3] * timestep0 ω0 = 100.0 * [1, 2, 3.0] * timestep0 -q10 = UnitQuaternion(RotX(0.5*π)) +q10 = Dojo.RotX(0.5*π) initialize!(mech, :snake, base_orientation=q10, @@ -280,7 +280,7 @@ end v0 = 10.0 * [1, 2, 3] * timestep0 ω0 = 10.0 * [1, 2, 3.0] * timestep0 - q10 = UnitQuaternion(RotX(0.5 * π)) + q10 = Dojo.RotX(0.5 * π) initialize!(mech, :snake, base_orientation=q10, @@ -330,7 +330,7 @@ mech = get_mechanism(:twister, v0 = 100.0 * [1, 2, 3] * timestep0 ω0 = 100.0 * [1, 2, 3.0] * timestep0 -q10 = UnitQuaternion(RotX(0.5*π)) +q10 = Dojo.RotX(0.5*π) initialize!(mech, :twister, base_orientation=q10, @@ -367,7 +367,7 @@ end v0 = 10.0 * [1, 2, 3] * timestep0 ω0 = 10.0 * [1, 2, 3.0] * timestep0 - q10 = UnitQuaternion(RotX(0.5*π)) + q10 = Dojo.RotX(0.5*π) initialize!(mech, :twister, base_orientation=q10, base_linear_velocity=v0, diff --git a/test/mrp.jl b/test/mrp.jl index de57054df..b78839257 100644 --- a/test/mrp.jl +++ b/test/mrp.jl @@ -1,5 +1,5 @@ @testset "Modified Rodrigues Parameters" begin - q = rand(UnitQuaternion) + q = rand(QuatRotation).q @test norm(Dojo.dmrpdq(Dojo.vector(q)) - FiniteDiff.finite_difference_jacobian(Dojo.mrp, Dojo.vector(q)), Inf) < 1.0e-5 @test norm(Dojo.daxisdq(Dojo.vector(q)) - @@ -7,7 +7,7 @@ @test norm(Dojo.drotation_vectordq(Dojo.vector(q)) - FiniteDiff.finite_difference_jacobian(Dojo.rotation_vector, Dojo.vector(q))) < 1.0e-5 - q = one(UnitQuaternion) + q = one(Quaternion{Float64}) @test norm(Dojo.dmrpdq(Dojo.vector(q)) - FiniteDiff.finite_difference_jacobian(Dojo.mrp, Dojo.vector(q)), Inf) < 1.0e-5 @test norm(Dojo.drotation_vectordq(Dojo.vector(q)) - diff --git a/test/runtests.jl b/test/runtests.jl index d1029410b..751789f55 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -4,6 +4,7 @@ using LinearAlgebra using Random using SparseArrays using BenchmarkTools +using Rotations using Dojo using Test