Get underlying quaternion parameters via Rotations.params(q)

Author: ferrolho

src/joint_types/quaternion_floating.jl

@@ -34,10 +34,11 @@ end
 @propagate_inbounds function set_rotation!(q::AbstractVector, jt::QuaternionFloating, rot::Rotation{3})
     T = eltype(rot)
     quat = convert(QuatRotation{T}, rot)
-    q[1] = quat.q.s
-    q[2] = quat.q.v1
-    q[3] = quat.q.v2
-    q[4] = quat.q.v3
+    w, x, y, z = Rotations.params(quat)
+    q[1] = w
+    q[2] = x
+    q[3] = y
+    q[4] = z
     nothing
 end
 

src/joint_types/quaternion_spherical.jl

@@ -30,10 +30,11 @@ end
 @propagate_inbounds function set_rotation!(q::AbstractVector, jt::QuaternionSpherical, rot::Rotation{3})
     T = eltype(rot)
     quat = convert(QuatRotation{T}, rot)
-    q[1] = quat.q.s
-    q[2] = quat.q.v1
-    q[3] = quat.q.v2
-    q[4] = quat.q.v3
+    w, x, y, z = Rotations.params(quat)
+    q[1] = w
+    q[2] = x
+    q[3] = y
+    q[4] = z
     nothing
 end
 

src/spatial/util.jl

@@ -125,11 +125,12 @@ function spquat_derivative end
 function angular_velocity_in_body end
 
 @inline function velocity_jacobian(::typeof(quaternion_derivative), q::QuatRotation)
+    w, x, y, z = Rotations.params(q)
     (@SMatrix [
-        -q.q.v1 -q.q.v2 -q.q.v3;
-         q.q.s  -q.q.v3  q.q.v2;
-         q.q.v3  q.q.s  -q.q.v1;
-        -q.q.v2  q.q.v1  q.q.s ]) / 2
+        -x -y -z;
+         w -z  y;
+         z  w -x;
+        -y  x  w ]) / 2
 end
 
 @inline function velocity_jacobian(::typeof(spquat_derivative), q::ModifiedRodriguesParam)
@@ -140,10 +141,11 @@ end
 end
 
 @inline function velocity_jacobian(::typeof(angular_velocity_in_body), q::QuatRotation)
+    w, x, y, z = Rotations.params(q)
     2 * @SMatrix [
-    -q.q.v1  q.q.s   q.q.v3 -q.q.v2;
-    -q.q.v2 -q.q.v3  q.q.s   q.q.v1;
-    -q.q.v3  q.q.v2 -q.q.v1  q.q.s ]
+    -x  w  z -y;
+    -y -z  w  x;
+    -z  y -x  w ]
 end
 
 @inline function velocity_jacobian(::typeof(angular_velocity_in_body), q::ModifiedRodriguesParam)

src/util.jl

@@ -44,7 +44,10 @@ end
 ## VectorSegment: type of a view of a vector
 const VectorSegment{T} = SubArray{T,1,Array{T, 1},Tuple{UnitRange{Int64}},true} # TODO: a bit too specific
 
-quatnorm(quat::QuatRotation) = sqrt(quat.q.s^2 + quat.q.v1^2 + quat.q.v2^2 + quat.q.v3^2)
+function quatnorm(quat::QuatRotation)
+    w, x, y, z = Rotations.params(quat)
+    sqrt(w^2 + x^2 + y^2 + z^2)
+end
 
 
 ## Modification count stuff

test/test_mechanism_algorithms.jl

@@ -905,8 +905,9 @@ end
             elseif joint_type_k isa QuaternionFloating || joint_type_k isa QuaternionSpherical
                 rot_orig = rotation(joint_type_k, q_orig)
                 rot_prin = rotation(joint_type_k, q_prin)
+                w, x, y, z = Rotations.params(rot_prin)
                 @test isapprox(rot_orig, rot_prin)
-                @test rot_prin.q.s > 0
+                @test w > 0
             else
                 @test q_orig == q_prin
             end