Remove Rotations.jl dependency

janbruedigam · janbruedigam · commit 8ef741e84058 · 2022-03-10T19:29:42.000+01:00
diff --git a/Project.toml b/Project.toml
@@ -20,7 +20,6 @@ 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"
@@ -41,7 +40,6 @@ 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"
-Rotations = "1.1"
 Scratch = "1.1"
 StaticArrays = "1.2"
 julia = "1.6, 1.7"
diff --git 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=Quaternion(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/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=Quaternion(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=Quaternion(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=Quaternion(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
@@ -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=Quaternion(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
@@ -25,8 +25,8 @@ function get_humanoid(;
 
 		aa = -0.43000 * [-0.44721, 0.00000, 0.89442]
 		ql = axis_angle_to_quaternion(aa)
-        qll = ql * Quaternion(RotXYZ(roll=-1.57080, pitch=1.47585, yaw=-1.47585)) # roll pitch yaw
-        qlr = ql * Quaternion(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
@@ -55,7 +55,7 @@ function initialize_orbital!(mechanism::Mechanism{T};
     # set position and velocities
     set_maximal_configurations!(mechanism.origin, pbody, 
         child_vertex=vert11, 
-        Δq=Quaternion(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
@@ -106,7 +106,7 @@ function initialize_npendulum!(mechanism::Mechanism{T};
     # set position and velocities
     set_maximal_configurations!(mechanism.origin, pbody,
         child_vertex=vert11,
-        Δq=Quaternion(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
@@ -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
@@ -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=Quaternion(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
@@ -70,7 +70,7 @@ end
 
 function initialize_snake!(mechanism::Mechanism{T};
     base_position=[0.0, -0.5, 0.0],
-    base_orientation=Quaternion(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/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=Quaternion(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/system_identification/methods/utils.jl b/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(QuatRotation).q, :ω => ω)
+	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
@@ -8,7 +8,6 @@ using Random
 using StaticArrays
 using SparseArrays
 using StaticArrays: SUnitRange
-using Rotations
 using Quaternions
 using Parameters
 using Statistics
@@ -257,10 +256,6 @@ export
 # Orientation
 export
     Quaternion,
-    QuatRotation,
-    RotX,
-    RotY,
-    RotZ,
     attitude_jacobian
 
 # Data
diff --git a/src/mechanism/urdf.jl b/src/mechanism/urdf.jl
@@ -51,7 +51,7 @@ function parse_pose(xpose, T)
     else
         x = parse_vector(xpose, "xyz", T, default = "0 0 0")
         rpy = parse_vector(xpose, "rpy", T, default = "0 0 0")
-        q = Quaternion(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
diff --git a/src/orientation/quaternion.jl b/src/orientation/quaternion.jl
@@ -1,8 +1,10 @@
-Quaternions.Quaternion(q::Rotation) = Quaternion(QuatRotation(q))
-# Base.rand(::Type{Quaternion}) = rand(Quaternion{Float64})
-MeshCat.LinearMap(q::Quaternion) = MeshCat.LinearMap(QuatRotation(q))
+MeshCat.LinearMap(q::Quaternion) = MeshCat.LinearMap(rotation_matrix(q))
 MeshCat.js_quaternion(q::Quaternion) = [q.v1, q.v2, q.v3, q.s]
 
+RotX(θ) = Quaternion(cos(θ/2), sin(θ/2), 0, 0)
+RotY(θ) = Quaternion(cos(θ/2), 0, sin(θ/2), 0)
+RotZ(θ) = Quaternion(cos(θ/2), 0, 0, sin(θ/2))
+
 quateltype(x) = eltype(x) # TODO not super elegant
 quateltype(::Quaternion{T}) where T = T
 
diff --git 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/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
@@ -480,7 +480,7 @@ mech = get_mechanism(:snake,
 
 v0 = 10.0 * [1, 2, 3] * timestep0
 ω0 = 10.0 * [1, 2, 3] * timestep0
-q10 = Quaternion(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 = Quaternion(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 = Quaternion(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 = Quaternion(RotX(0.5*π))
+        q10 = Dojo.RotX(0.5*π)
 
         initialize!(mech, :twister, 
             base_orientation=q10, 
diff --git 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 = Quaternion(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 = Quaternion(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 = Quaternion(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 = Quaternion(RotX(0.5*π))
+        q10 = Dojo.RotX(0.5*π)
         initialize!(mech, :twister, 
             base_orientation=q10, 
             base_linear_velocity=v0, 
diff --git 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
