Switch from Rotations.kl to Quaternions.jl

Author: janbruedigam

Project.toml

@@ -18,6 +18,7 @@ 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"
@@ -39,7 +40,7 @@ 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"
-Rotations = "1.1"
+Quaternions = "<0.5.3, 0.5.3"
 Scratch = "1.1"
 StaticArrays = "1.2"
 julia = "1.6, 1.7"

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
 

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=Quaternion(RotX(orientation)))
     set_maximal_velocities!(body, 
         v=[0.0; linear_velocity], 
         ω=[angular_velocity, 0.0, 0.0])

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]))

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=Quaternion(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=Quaternion(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=Quaternion(RotX(π)))
         set_maximal_velocities!(mech.bodies[2], 
             v=zeros(3), 
             ω=zeros(3))

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=Quaternion(RotY(0.5 * π)),
         color=color, name=:side)
     links = [main_body, side_body]
 

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 * 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
 
         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)

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=Quaternion(RotX(0.0)))
 
     set_minimal_coordinates!(mechanism, mechanism.joints[2], orientation)
 

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=Quaternion(RotX(base_angle)))
     set_maximal_velocities!(pbody,
         ω=base_angular_velocity)
 

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=Quaternion(RotX(0.0)))
     set_maximal_velocities!(pbody, cbody, 
         parent_vertex=tra2.vertices[1], 
         child_vertex=tra2.vertices[2],