Cleanup

Author: janbruedigam

examples/system_identification/methods/utils.jl

@@ -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(Quaternion), :ω => ω)
+	return Dict(:x => x, :v => v , :q => rand(QuatRotation).q, :ω => ω)
 end
 
 function build_pairs(mechanism::Mechanism, trajs::AbstractVector)

src/Dojo.jl

@@ -9,7 +9,6 @@ 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

src/orientation/quaternion.jl

@@ -1,23 +1,6 @@
-# Quaternion(w::T, v::StaticVector{3,T}, normalize::Bool = true) where T = Quaternion{T}(w, v[1], v[2], v[3], normalize)
-# Quaternion(w::T, v::Vector{T}, normalize::Bool = true) where T = (@assert length(v)==3; Quaternion{T}(w, v[1], v[2], v[3], normalize))
-# Quaternion(v::StaticVector{3,T}) where T = pure_quaternion(v)
-# Quaternion(v::Vector) = (@assert length(v)==3; pure_quaternion(v))
-
-# imag(q::Quaternion) = vector(q)
-
-# Remove once added to Rotations.jl
-# function Base.:/(q::Quaternion, w::Real)
-#     return Quaternion(q.s/w, q.v1/w, q.v2/w, q.v3/w, false)
-# end
-
-MeshCat.LinearMap(q::Quaternion) = MeshCat.LinearMap(QuatRotation(q))
-MeshCat.js_quaternion(q::Quaternion) = [q.v1, q.v2, q.v3, q.s]
-
-quateltype(x) = eltype(x)
+quateltype(x) = eltype(x) # TODO not super elegant
 quateltype(::Quaternion{T}) where T = T
 
-Base.rand(::Type{Quaternion}) = Quaternion(rand(QuatRotation))
-
 Quaternion(q::Rotation) = Quaternion(QuatRotation(q))
 
 vector(q::Quaternion) = SA[q.s, q.v1, q.v2, q.v3]
@@ -230,4 +213,9 @@ end
 
 function ∂skew∂p(λ) # 𝞉(skew(p)*λ)/∂p
 	skew(-λ)
-end
+end
+
+
+# MeshCat fixes
+MeshCat.LinearMap(q::Quaternion) = MeshCat.LinearMap(QuatRotation(q))
+MeshCat.js_quaternion(q::Quaternion) = [q.v1, q.v2, q.v3, q.s]

test/impulse_map.jl

@@ -5,13 +5,13 @@
     mech = Dojo.get_mechanism(:pendulum)
     joint0 = mech.joints[1]
     rot0 = joint0.rotational
-    rot0.axis_offset = rand(Quaternion)
+    rot0.axis_offset = rand(QuatRotation).q
 
 
     xa = rand(3)
-    qa = rand(Quaternion)
+    qa = rand(QuatRotation).q
     xb = rand(3)
-    qb = rand(Quaternion)
+    qb = rand(QuatRotation).q
 
     # displacement_jacobian_configuration
     X0, Q0 = Dojo.displacement_jacobian_configuration(:parent, rot0, xa, qa, xb, qb, 
@@ -42,9 +42,9 @@ end
     tra0 = joint0.translational
 
     xa = rand(3)
-    qa = rand(Quaternion)
+    qa = rand(QuatRotation).q
     xb = rand(3)
-    qb = rand(Quaternion)
+    qb = rand(QuatRotation).q
 
     # displacement_jacobian_configuration
     X0, Q0 = Dojo.displacement_jacobian_configuration(:parent, tra0, xa, qa, xb, qb, 
@@ -77,12 +77,12 @@ end
     mech = Dojo.get_mechanism(:pendulum)
     joint0 = mech.joints[1]
     rot0 = joint0.rotational
-    rot0.axis_offset = rand(Quaternion)
+    rot0.axis_offset = rand(QuatRotation).q
 
     xa = rand(3)
-    qa = rand(Quaternion)
+    qa = rand(QuatRotation).q
     xb = rand(3)
-    qb = rand(Quaternion)
+    qb = rand(QuatRotation).q
 
     p0 = rand(3)
     J0 = Dojo.impulse_transform_jacobian(:parent, :parent, rot0, xa, qa, xb, qb, p0)
@@ -124,9 +124,9 @@ end
     tra0 = joint0.translational
 
     xa = rand(3)
-    qa = rand(Quaternion)
+    qa = rand(QuatRotation).q
     xb = rand(3)
-    qb = rand(Quaternion)
+    qb = rand(QuatRotation).q
 
     p0 = rand(3)
     J0 = Dojo.impulse_transform_jacobian(:parent, :parent, tra0, xa, qa, xb, qb, p0)
@@ -172,9 +172,9 @@ end
     tra0 = joint0.translational
 
     xa = rand(3)
-    qa = rand(Quaternion)
+    qa = rand(QuatRotation).q
     xb = rand(3)
-    qb = rand(Quaternion)
+    qb = rand(QuatRotation).q
     λ = rand(3)
     η = rand(0)
     Dojo.impulse_map(:parent, tra0, xa, qa, xb, qb, η)
@@ -230,13 +230,13 @@ end
     mech = Dojo.get_mechanism(:pendulum)
     joint0 = mech.joints[1]
     rot0 = joint0.rotational
-    rot0.axis_offset = rand(Quaternion)
+    rot0.axis_offset = rand(QuatRotation).q
 
 
     xa = rand(3)
-    qa = rand(Quaternion)
+    qa = rand(QuatRotation).q
     xb = rand(3)
-    qb = rand(Quaternion)
+    qb = rand(QuatRotation).q
     λ = rand(2)
     η = rand(0)
     Dojo.impulse_map(:parent, rot0, xa, qa, xb, qb, η)

test/integrator.jl

@@ -2,7 +2,7 @@
     Random.seed!(100)
     x0 = srand(3)
     v0 = srand(3)
-    q0 = rand(Quaternion)
+    q0 = rand(QuatRotation).q
     ϕ0 = srand(3)
     timestep0 = 0.01
     x1 = Dojo.next_position(x0, v0, timestep0)

test/minimal.jl

@@ -35,7 +35,7 @@ joint_types = [
 
 	x = srand(1)
 	Δx = Dojo.zerodimstaticadjoint(Dojo.nullspace_mask(tra2)) * x
-	Δq = rand(Quaternion)
+	Δ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 = rand(Quaternion)
+		    joint.rotational.axis_offset = rand(QuatRotation).q
 		end
 	    joint0 = mech.joints[1]
 	    tra0 = joint0.translational

test/mrp.jl

@@ -1,5 +1,5 @@
 @testset "Modified Rodrigues Parameters" begin
-    q = rand(Quaternion)
+    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)) -