Skip to content

Commit

Permalink
Finish adding attitude transformations.
Browse files Browse the repository at this point in the history
  • Loading branch information
@duncaneddy
duncaneddy committed Jan 3, 2019
1 parent a47bd2f commit 46981f3
Show file tree
Hide file tree
Showing 4 changed files with 482 additions and 53 deletions.
8 changes: 4 additions & 4 deletions Manifest.toml
View file
Expand Up @@ -6,9 +6,9 @@ version = "0.3.0"

[[BandedMatrices]]
deps = ["FillArrays", "LazyArrays", "LinearAlgebra", "Random", "SparseArrays", "Test"]
git-tree-sha1 = "04a5e589951a6051669560485aa41410201ddec3"
git-tree-sha1 = "3804222a671e7e4f697cc31b6639592ba8ae00ae"
uuid = "aae01518-5342-5314-be14-df237901396f"
version = "0.8.0"
version = "0.8.1"

[[Base64]]
uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
Expand Down Expand Up @@ -569,9 +569,9 @@ version = "0.7.2"

[[StaticArrays]]
deps = ["InteractiveUtils", "LinearAlgebra", "Random", "Statistics", "Test"]
git-tree-sha1 = "97c4bf0f647488dd7ac01ea12be5885f88762938"
git-tree-sha1 = "1eb114d6e23a817cd3e99abc3226190876d7c898"
uuid = "90137ffa-7385-5640-81b9-e52037218182"
version = "0.10.0"
version = "0.10.2"

[[Statistics]]
deps = ["LinearAlgebra", "SparseArrays"]
Expand Down
240 changes: 193 additions & 47 deletions src/attitude.jl
View file
Expand Up @@ -118,7 +118,8 @@ mutable struct Quaternion

function Quaternion(q0::Real, q1::Real, q2::Real, q3::Real)
n = sqrt(q0^2 + q1^2 + q2^2 + q3^2)
new(q0/n, q1/n, q2/n, q3/n)

return new(q0/n, q1/n, q2/n, q3/n)
end
end

Expand All @@ -127,7 +128,7 @@ export EulerAngle
The `EulerAngle` type provides a represenation of EulerAngles for storing attitude
information.
Valid sequences are: `:121, :123, :131, :132, :212, :213, :231, :232, :312, :313, :321, :323`.
Valid sequences are: `121, 123, 131, 132, 212, 213, 231, 232, 312, 313, 321, 323`.
Data members:
- `seq::Symbol`: Order of application of angles with respect to body axis.
Expand All @@ -143,6 +144,14 @@ mutable struct EulerAngle
phi::Float64
theta::Float64
psi::Float64

function EulerAngle(seq::Integer, phi::Real, theta::Real, psi::Real)
if !(seq in [121, 123, 131, 132, 212, 213, 231, 232, 312, 313, 321, 323])
throw(ArgumentError("Invalid EulerAngle sequence: $seq"))
end

return new(seq, phi, theta, psi)
end
end

export EulerAxis
Expand All @@ -160,6 +169,14 @@ References:
mutable struct EulerAxis
angle::Float64
axis::Array{Float64, 1}

function EulerAxis(angle::Real, axis::Array{<:Real, 1})
if length(axis) != 3
throw(ArgumentError("Invalid array for EulerAxis initialization. Input size: $(size(axis)), Required size: (3,)"))
end

return new(angle, axis)
end
end

##############
Expand Down Expand Up @@ -310,7 +327,7 @@ function Quaternion(e::EulerAngle)
else
# Should get an invalid sequence, but it is possible if a user
# Directly sets the sequence number
throw(ArgumentError("Invalid EulerAngle sequence: $seq"))
throw(ArgumentError("Invalid EulerAngle sequence: $e.seq"))
end

return Quaternion(q0, q1, q2, q3)
Expand Down Expand Up @@ -348,11 +365,11 @@ function Base.getindex(q::Quaternion, I::Integer)
elseif I == 4
return q.q3
else
throw(BoundsError([as_vector(q)],[i]))
throw(BoundsError())
end
end

Base.getindex(q::Quaternion, ::Colon) = [q.q0 q.q1 q.q2 q.q3]
Base.getindex(q::Quaternion, ::Colon) = [q.q0, q.q1, q.q2, q.q3]

# Return quaternion as a vector
export as_vector
Expand Down Expand Up @@ -548,11 +565,11 @@ function slerp(q0::Quaternion, q1::Quaternion, t::Real)
end

# Extract vectors and normalize
q0 = copy(q0)
q1 = copy(q1)
q0 = copy(q0)[:]
q1 = copy(q1)[:]

# Compute cosine of the angle between the two vectors
dp = dot(q0[:], q1[:])
dp = dot(q0, q1)

# If the dot product is negative, the quaternions have opposite handed-ness
# and slerp won't take the shortest path. Fix by reversing one quaternion.
Expand All @@ -579,112 +596,241 @@ end
# EulerAngle #
##############

function EulerAngle(seq::Integer, vec::Array{<:Real, 1})
if length(vec) != 3
throw(ArgumentError("Invalid array for EulerAngle initialization. Input length: $(length(vec)), Required length: 3"))
end

EulerAngle(seq, vec...)
end

function EulerAngle(seq::Integer, mat::Array{<:Real, 2})
if size(mat) != (3,3)
throw(ArgumentError("Invalid array for Quaternion initialization. Input size: $(size(mat)), Required size: (3,3)"))
end

# Extract elements out of rotation matrix
r11 = rot[1, 1]
r12 = rot[1, 2]
r13 = rot[1, 3]
r21 = rot[2, 1]
r22 = rot[2, 2]
r23 = rot[2, 3]
r31 = rot[3, 1]
r32 = rot[3, 2]
r33 = rot[3, 3]
r11 = mat[1, 1]
r12 = mat[1, 2]
r13 = mat[1, 3]
r21 = mat[2, 1]
r22 = mat[2, 2]
r23 = mat[2, 3]
r31 = mat[3, 1]
r32 = mat[3, 2]
r33 = mat[3, 3]

# Select euler angle sequence
phi, theta, psi = 0.0, 0.0, 0.0
if seq == 121
phi = atan2(r21, r31)
phi = atan(r21, r31)
theta = acos(r11)
psi = atan2(r12, -r13)
psi = atan(r12, -r13)
elseif seq == 123
phi = atan2(r23, r33)
phi = atan(r23, r33)
theta = -asin(r13)
psi = atan2(r12, r11)
psi = atan(r12, r11)
elseif seq == 131
phi = atan2(r31, -r21)
phi = atan(r31, -r21)
theta = acos(r11)
psi = atan2(r13, r12)
psi = atan(r13, r12)
elseif seq == 132
phi = atan2(-r32, r22)
phi = atan(-r32, r22)
theta = asin(r12)
psi = atan2(-r13, r11)
psi = atan(-r13, r11)
elseif seq == 212
phi = atan2(r12, -r32)
phi = atan(r12, -r32)
theta = acos(r22)
psi = atan2(r21, r23)
psi = atan(r21, r23)
elseif seq == 213
phi = atan2(-r13, r33)
phi = atan(-r13, r33)
theta = asin(r23)
psi = atan2(-r21, r22)
psi = atan(-r21, r22)
elseif seq == 231
phi = atan2(r31, r11)
phi = atan(r31, r11)
theta = -asin(r21)
psi = atan2(r23, r22)
psi = atan(r23, r22)
elseif seq == 232
phi = atan2(r32, r12)
phi = atan(r32, r12)
theta = acos(r22)
psi = atan2(r23, -r21)
psi = atan(r23, -r21)
elseif seq == 312
phi = atan2(r12, r22)
phi = atan(r12, r22)
theta = -asin(r32)
psi = atan2(r31, r33)
psi = atan(r31, r33)
elseif seq == 313
phi = atan2(r13, r23)
phi = atan(r13, r23)
theta = acos(r33)
psi = atan2(r31, -r32)
psi = atan(r31, -r32)
elseif seq == 321
phi = atan2(-r21, r11)
phi = atan(-r21, r11)
theta = asin(r31)
psi = atan2(-r32, r33)
psi = atan(-r32, r33)
elseif seq == 323
phi = atan2(r23, -r13)
phi = atan(r23, -r13)
theta = acos(r33)
psi = atan2(r32, r31)
psi = atan(r32, r31)
else
throw(ArgumentError("Invalid EulerAngle sequence: $seq"))
end

return EulerAngle(seq, phi, theta, psi)
end

function EulerAngle(q::Quaternion)
function EulerAngle(seq::Integer, q::Quaternion)
# Construct angle from Quaternion by going through a rotation matrix
return EulerAngle(as_matrix(q))
return EulerAngle(seq::Integer, as_matrix(q))
end

function EulerAngle(e::EulerAxis)
function EulerAngle(seq::Integer, ea::EulerAxis)
# Construct angle from EulerAxis by going through a rotation matrix
return EulerAngle(as_matrix(q))
return EulerAngle(seq::Integer, as_matrix(ea))
end

# Access Operators
function Base.getindex(e::EulerAngle, I::UnitRange{<:Integer})
# Allocate vector once
vec = as_vector(e)

# Return selected index or range
return [vec[i] for i in I]
end

function Base.getindex(e::EulerAngle, I::Integer)
if I == 1
return e.phi
elseif I == 2
return e.theta
elseif I == 3
return e.psi
else
throw(BoundsError())
end
end

Base.getindex(e::EulerAngle, ::Colon) = [e.phi, e.theta, e.psi]

"""
Return Euler angles as a vector.
Equivalent to: `[e.phi, e.theta, e.psi]` for `EulerAngle` `e`
Arguments:
- `e::EulerAngle` Euler Angle
Returns:
- `evec::Array{Float64, 1}` Euler angles components in vector form.
"""
function as_vector(e::EulerAngle)
return [e.phi, e.theta, e.psi]
end

function as_matrix(e::EulerAngle)
# Get EulerAngle as matrix by going through Quaternions
return as_matrix(Quaternion(e))
end

function Base.copy(e::EulerAngle)
return EulerAngle(e.seq, e.phi, e.theta, e.psi)
end

function Base.deepcopy(e::EulerAngle)
return EulerAngle(e.seq, e.phi, e.theta, e.psi)
end

#############
# EulerAxis #
#############

function EulerAxis(angle::Real, v1::Real, v2::Real, v3::Real)
return EulerAxis(angle, [v1, v2, v3])
end

function EulerAxis(vec::Array{<:Real, 1})
if length(vec) != 4
throw(ArgumentError("Invalid array for EulerAxis initialization. Input size: $(size(vec)), Required size: (4,)"))
end

return EulerAxis(vec[1], [vec[2], vec[3], vec[4]])
end

function EulerAxis(mat::Array{<:Real, 2})
if size(mat) != (3,3)
throw(ArgumentError("Invalid array for Quaternion initialization. Input size: $(size(mat)), Required size: (3,3)"))
throw(ArgumentError("Invalid array for EulerAxis initialization. Input size: $(size(mat)), Required size: (3,3)"))
end

return EulerAxis(Quaternion(mat))
end

function EulerAxis(q::Quaternion)
# Extract quaternion vector and normalize
qv = as_vector(q)
q = qv/norm(qv)

# Ensure first element is positive
if q[1] < 0
q = -q
end

# Compute Euler Angle
angle = 2*acos(q[1])
qvec_norm = norm(q[2:4])
vec = [0.0, 0.0, 0.0]

if qvec_norm > 1e-15
vec = q[2:4]/qvec_norm
end

return EulerAxis(angle, vec)
end

function EulerAxis(e::EulerAngle)
# If input is an EulerAngle first compute a quaternion then get the
# EulerAxis form
EulerAxis(Quaternion(e))
end

# Access operators

function Base.getindex(e::EulerAxis, I::UnitRange{<:Integer})
# Allocate vector once
vec = as_vector(e)

# Return selected index or range
return [vec[i] for i in I]
end

function Base.getindex(e::EulerAxis, I::Integer)
if I == 1
return e.angle
elseif I == 2
return e.axis[1]
elseif I == 3
return e.axis[2]
elseif I == 4
return e.axis[3]
else
throw(BoundsError())
end
end

function EulerAxis(e::EulerAxis)
Base.getindex(e::EulerAxis, ::Colon) = [e.angle, e.axis[1], e.axis[2], e.axis[3]]

function as_vector(e::EulerAxis)
return e[:]
end

function as_matrix(e::EulerAxis)
# Get matrix form from Quaternion for ease
return as_matrix(Quaternion(e))
end

function Base.copy(e::EulerAxis)
return EulerAxis(e.angle, e.axis)
end

function Base.deepcopy(e::EulerAxis)
return EulerAxis(e.angle, e.axis)
end

####################
Expand Down

0 comments on commit 46981f3

Please sign in to comment.