diff --git a/Project.toml b/Project.toml
index 8f8ac80..ad888ed 100644
--- a/Project.toml
+++ b/Project.toml
@@ -1,13 +1,14 @@
 name = "RobotDynamics"
 uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1"
 authors = ["Brian Jackson <bjack205@gmail.com>"]
-version = "0.4.7"
+version = "0.4.8"
 
 [deps]
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
+Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0"
 RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
@@ -17,6 +18,7 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 FiniteDiff = "2.8"
 ForwardDiff = "0.10"
 RecipesBase = "1"
-Rotations = "~1.0"
+Rotations = "1.0"
 StaticArrays = "1"
+Quaternions = "0.7"
 julia = "1"
diff --git a/README.md b/README.md
index a72d0e3..f2d8fa5 100644
--- a/README.md
+++ b/README.md
@@ -116,7 +116,7 @@ end
 
 # Build model
 T = Float64
-R = UnitQuaternion{T}
+R = QuatRotation{T}
 mass = 1.0
 J = Diagonal(@SVector ones(3))
 model = Satellite{R,T}(mass, J)
diff --git a/examples/satellite_pair.jl b/examples/satellite_pair.jl
index fc9fa16..16e121b 100644
--- a/examples/satellite_pair.jl
+++ b/examples/satellite_pair.jl
@@ -19,10 +19,10 @@ function RobotDynamics.dynamics(model::SatellitePair, x, u)
     q2 = qs[2]
 
     J1, J2 = model.J1, model.J2
-    u1 = u[SA[1,2,3]]
-    u2 = u[SA[4,5,6]]
-    ω1dot = J1\(u1 - ω1 × (J1 * ω1))
-    ω2dot = J2\(u2 - ω2 × (J2 * ω2))
+    u1 = u[SA[1, 2, 3]]
+    u2 = u[SA[4, 5, 6]]
+    ω1dot = J1 \ (u1 - ω1 × (J1 * ω1))
+    ω2dot = J2 \ (u2 - ω2 × (J2 * ω2))
     q1dot = Rotations.kinematics(q1, ω1)
     q2dot = Rotations.kinematics(q2, ω2)
     SA[
@@ -35,12 +35,12 @@ end
 
 RobotDynamics.control_dim(::SatellitePair) = 6
 
-RobotDynamics.LieState(::SatellitePair{R}) where R = RobotDynamics.LieState(R, (0,3,3))
+RobotDynamics.LieState(::SatellitePair{R}) where {R} = RobotDynamics.LieState(R, (0, 3, 3))
 
 J1 = SMatrix{3,3}(Diagonal(fill(1.0, 3)))
 J2 = SMatrix{3,3}(Diagonal(fill(2.0, 3)))
-model = SatellitePair{UnitQuaternion{Float64}, Float64}(J1, J2)
-x,u = rand(model)
+model = SatellitePair{QuatRotation{Float64},Float64}(J1, J2)
+x, u = rand(model)
 @test norm(x[1:4]) ≈ 1
 @test norm(x[8:11]) ≈ 1
 s = RobotDynamics.LieState(model)
@@ -48,14 +48,14 @@ s = RobotDynamics.LieState(model)
 
 ω1 = x[5:7]
 ω2 = x[12:14]
-q1 = UnitQuaternion(x[1:4],false)
-q2 = UnitQuaternion(x[8:11],false)
+q1 = QuatRotation(x[1:4], false)
+q2 = QuatRotation(x[8:11], false)
 @test all(RobotDynamics.vec_states(s, x)[2:3] .≈ (ω1, ω2))
-@test all(RobotDynamics.rot_states(s, x) .≈ (q1,q2))
+@test all(RobotDynamics.rot_states(s, x) .≈ (q1, q2))
 
 
-model = SatellitePair{MRP{Float64}, Float64}(J1, J2)
-x,u = rand(model)
+model = SatellitePair{MRP{Float64},Float64}(J1, J2)
+x, u = rand(model)
 @test length(x) == 12
 s = RobotDynamics.LieState(model)
 @test length(s) == 12
@@ -65,4 +65,4 @@ s = RobotDynamics.LieState(model)
 q1 = MRP(x[1:3]...)
 q2 = MRP(x[7:9]...)
 @test all(RobotDynamics.vec_states(s, x)[2:3] .≈ (ω1, ω2))
-@test all(RobotDynamics.rot_states(s, x) .≈ (q1,q2))
+@test all(RobotDynamics.rot_states(s, x) .≈ (q1, q2))
diff --git a/examples/single_satellite.jl b/examples/single_satellite.jl
index 62f0eb1..3ab7aa3 100644
--- a/examples/single_satellite.jl
+++ b/examples/single_satellite.jl
@@ -18,7 +18,7 @@ RobotDynamics.inertia(model::Satellite) = model.J
 function RobotDynamics.forces(model::Satellite, x::StaticVector, u::StaticVector)
     q = orientation(model, x)
     F = @SVector [u[1], u[2], u[3]]
-    q*F  # world frame
+    q * F  # world frame
 end
 
 # Define the 3D moments at the center of mass, in the body frame
@@ -29,14 +29,14 @@ end
 
 # Build model
 T = Float64
-R = UnitQuaternion{T}
+R = QuatRotation{T}
 mass = 1.0
 J = Diagonal(@SVector ones(3))
 model = Satellite{R,T}(mass, J)
 
 # Initialization
-x,u = rand(model)
-z = KnotPoint(x,u,0.1)
+x, u = rand(model)
+z = KnotPoint(x, u, 0.1)
 ∇f = RobotDynamics.DynamicsJacobian(model)
 
 # Continuous dynamics
@@ -51,8 +51,8 @@ function RobotDynamics.wrench_jacobian!(F, model::Satellite, z)
     u = control(z)
     q = orientation(model, x)
     ir, iq, iv, iω, iu = RobotDynamics.gen_inds(model)
-    iF = SA[1,2,3]
-    iM = SA[4,5,6]
+    iF = SA[1, 2, 3]
+    iM = SA[4, 5, 6]
     F[iF, iq] .= Rotations.∇rotate(q, u[iF])
     F[iF, iu[iF]] .= RotMatrix(q)
     for i = 1:3
@@ -63,8 +63,8 @@ end
 b2 = @benchmark jacobian!($∇f, $model, $z)
 
 function RobotDynamics.wrench_sparsity(::Satellite)
-    SA[false true  false false true;
-       false false false false true]
+    SA[false true false false true;
+        false false false false true]
 end
 b3 = @benchmark jacobian!($∇f, $model, $z)
 println("Analytical Wrench Jacobian:  ", judge(minimum(b2), minimum(b1)))
diff --git a/src/RobotDynamics.jl b/src/RobotDynamics.jl
index d08e6d9..3dcd6ce 100644
--- a/src/RobotDynamics.jl
+++ b/src/RobotDynamics.jl
@@ -8,6 +8,7 @@ using FiniteDiff
 using RecipesBase
 using SparseArrays
 using Pkg
+using Quaternions
 
 using Rotations: skew
 using StaticArrays: SUnitRange
@@ -51,7 +52,7 @@ function FiniteDiff.finite_difference_gradient!(
     # c1 is x1 if we need a complex copy of x, otherwise Nothing
     # c2 is Nothing
     fx, c1, c2, c3 = cache.fx, cache.c1, cache.c2, cache.c3
-    copyto!(c3,x)
+    copyto!(c3, x)
     if fdtype == Val(:forward)
         for i ∈ eachindex(x)
             epsilon = compute_epsilon(fdtype, x[i], relstep, absstep, dir)
diff --git a/src/liestate.jl b/src/liestate.jl
index fd38fd1..10ff4ba 100644
--- a/src/liestate.jl
+++ b/src/liestate.jl
@@ -16,14 +16,14 @@ Note that this function should only be a function of the type, not the actual
 struct itself (i.e. your method definition should look like the one above).
 """
 abstract type LieGroupModel <: ContinuousDynamics end
-const DiscreteLieDynamics = DiscretizedDynamics{L,Q} where {L<:LieGroupModel, Q<:QuadratureRule}
+const DiscreteLieDynamics = DiscretizedDynamics{L,Q} where {L<:LieGroupModel,Q<:QuadratureRule}
 
 # RotationState interface
-statevectortype(::Type{<:LieGroupModel}) = RotationState() 
+statevectortype(::Type{<:LieGroupModel}) = RotationState()
 
 @inline errstate_dim(::RotationState, model::AbstractModel) = errstate_dim(LieState(model))
 
-@inline state_diff(::RotationState, model::AbstractModel, x::AbstractVector, 
+@inline state_diff(::RotationState, model::AbstractModel, x::AbstractVector,
     x0::AbstractVector, errmap=Rotations.CayleyMap()) =
     state_diff(LieState(model), x, x0, errmap)
 
@@ -39,7 +39,7 @@ statevectortype(::Type{<:LieGroupModel}) = RotationState()
 
 import Rotations.params
 
-params(::Type{<:UnitQuaternion}) = 4
+params(::Type{<:QuatRotation}) = 4
 params(::Type{<:RodriguesParam}) = 3
 params(::Type{<:MRP}) = 3
 params(::Type{<:RotMatrix3}) = 9
@@ -65,7 +65,7 @@ end of the state vector.
 If we want to construct a state vector like the following: `[v3, q, v2, q, v3]` where `v2`
 and `v3` and vector components of length 2 and 3, respectively, and `q` is a 4-dimensional
 unit quaternion. The `LieState` for this state vector would be
-`LieState{UnitQuaternion{Float64},3,2,3}`. The length should be (3+4+2+4+3) = 16, which can
+`LieState{QuatRotation{Float64},3,2,3}`. The length should be (3+4+2+4+3) = 16, which can
 be verified by `length(s::LieState)`.
 
 # Constructors
@@ -78,15 +78,15 @@ struct LieState{R,P}
     end
 end
 
-@inline LieState(::Type{R}, P::Tuple{Vararg{Int}}) where R <: Rotation = LieState{R,P}()
-@inline LieState(::Type{R}, P::Int...) where R <: Rotation = LieState{R,P}()
+@inline LieState(::Type{R}, P::Tuple{Vararg{Int}}) where {R<:Rotation} = LieState{R,P}()
+@inline LieState(::Type{R}, P::Int...) where {R<:Rotation} = LieState{R,P}()
 LieState(model::DiscreteLieDynamics) = LieState(model.continuous_dynamics)
 
 """
     QuatState(n::Int, Q::StaticVector{<:Any,Int})
     QuatState(n::Int, Q::NTuple{<:Any,Int})
 
-Create a `n`-dimensional `LieState` assuming `R = UnitQuaternion{Float64}` and `Q[i]` is the
+Create a `n`-dimensional `LieState` assuming `R = QuatRotation{Float64}` and `Q[i]` is the
 first index of each quaternion in the state vector.
 
 # Example
@@ -100,36 +100,36 @@ call `QuatState(16, SA[4,10])`.
     if NR == 1
         P0 = SVector{0,Int}()
     else
-        diffs = [:(Q[$i] - Q[$(i-1)] - 4) for i = 2:NR]
-        P0 = :(SVector{NR-1}(tuple($(diffs...))))
+        diffs = [:(Q[$i] - Q[$(i - 1)] - 4) for i = 2:NR]
+        P0 = :(SVector{NR - 1}(tuple($(diffs...))))
     end
     quote
         P = $P0
         P = push(P, n - (Q[end] + 4) + 1)
         P = pushfirst(P, Q[1] - 1)
-        return LieState(UnitQuaternion{Float64}, Tuple(P))
+        return LieState(QuatRotation{Float64}, Tuple(P))
     end
 end
 @inline QuatState(n::Int, Q::NTuple{<:Any,Int}) = QuatState(n, SVector(Q))
 
 "number of rotations"
-num_rotations(::LieState{<:Any,P}) where P = length(P) - 1
+num_rotations(::LieState{<:Any,P}) where {P} = length(P) - 1
 
-state_dim_vec(::LieState{<:Any,P}) where P = sum(P)
-state_dim_rot(s::LieState{R}) where R = params(R)*num_rotations(s)
+state_dim_vec(::LieState{<:Any,P}) where {P} = sum(P)
+state_dim_rot(s::LieState{R}) where {R} = params(R) * num_rotations(s)
 
-Base.length(s::LieState{R,P}) where {R,P} = params(R)*num_rotations(s) + sum(P)
-Base.length(::Type{LieState{R,P}}) where {R,P} = params(R)*(length(P)-1) + sum(P)
+Base.length(s::LieState{R,P}) where {R,P} = params(R) * num_rotations(s) + sum(P)
+Base.length(::Type{LieState{R,P}}) where {R,P} = params(R) * (length(P) - 1) + sum(P)
 
-errstate_dim(s::LieState{R,P}) where {R,P} = 3*num_rotations(s) + sum(P)
+errstate_dim(s::LieState{R,P}) where {R,P} = 3 * num_rotations(s) + sum(P)
 
 # Useful functions for meta-programming
-rot_inds(R,P, i::Int) = (sum(P[1:i]) + (i-1)*params(R)) .+ (1:params(R))
-vec_inds(R,P, i::Int) =
-    ((i > 1 ? sum(P[1:i-1]) : 0) + (i-1)*params(R)) .+ (1:P[i])
-inds(R,P, i::Int) = isodd(i) ? vec_inds(R,P, 1+i÷2) : rot_inds(R,P, i÷2)
+rot_inds(R, P, i::Int) = (sum(P[1:i]) + (i - 1) * params(R)) .+ (1:params(R))
+vec_inds(R, P, i::Int) =
+    ((i > 1 ? sum(P[1:i-1]) : 0) + (i - 1) * params(R)) .+ (1:P[i])
+inds(R, P, i::Int) = isodd(i) ? vec_inds(R, P, 1 + i ÷ 2) : rot_inds(R, P, i ÷ 2)
 @inline inds(s::LieState{R,P}, i::Int) where {R,P} = inds(R, P, i)
-rot_state(R,P, i::Int, sym=:x) = [:($(sym)[$j]) for j in rot_inds(R,P,i)]
+rot_state(R, P, i::Int, sym=:x) = [:($(sym)[$j]) for j in rot_inds(R, P, i)]
 
 """
     vec_states(model::LieGroupModel, x)
@@ -141,7 +141,7 @@ a tuple `v` of `SVector`s, where `length(v[i])` is equal to the length specified
 """
 @generated function vec_states(s::LieState{R,P}, x) where {R,P}
     T = eltype(x)
-    inds = [vec_inds(R,P,i) for i = 1:length(P)]
+    inds = [vec_inds(R, P, i) for i = 1:length(P)]
     states = [[:(x[$i]) for i in ind] for ind in inds]
     vecs = [:(SVector{$(length(inds)),$T}($(inds...))) for inds in states]
     quote
@@ -159,10 +159,10 @@ a tuple rotations, whose type matches the rotation type specified in the `LieSta
 """
 @generated function rot_states(s::LieState{R,P}, x) where {R,P}
     T = eltype(x)
-    inds = [rot_inds(R,P,i) for i = 1:length(P)-1]
+    inds = [rot_inds(R, P, i) for i = 1:length(P)-1]
     states = [[:(x[$i]) for i in ind] for ind in inds]
-    if R <: UnitQuaternion
-        vecs = [:(R($(inds...),false)) for inds in states]
+    if R <: QuatRotation
+        vecs = [:(R($(inds...), false)) for inds in states]
     else
         vecs = [:(R($(inds...))) for inds in states]
     end
@@ -182,13 +182,13 @@ end
     x = Expr[]
     for i = 1:np
         if isodd(i)  # vector part
-            vi = inds(R,P,i)
+            vi = inds(R, P, i)
             for j in vi
                 push!(x, :(rand()))
             end
         else
             r = i ÷ 2
-            ri = inds(R,P,i)
+            ri = inds(R, P, i)
             for j = 1:Rotations.params(R)
                 push!(x, :($(Symbol("q$r"))[$j]))
             end
@@ -207,14 +207,14 @@ end
 @inline state_dim(model::LieGroupModel) = length(LieState(model))
 
 
-function _state_diff_expr(R,P)
+function _state_diff_expr(R, P)
     nr = length(P) - 1   # number of rotations
     np = nr + length(P)  # number of partitions
-    n̄ = 3*nr + sum(P)    # error state size
+    n̄ = 3 * nr + sum(P)    # error state size
 
     # Generate a vector of δq = q0\q expressions for each q in the state
     dq = [:($(Symbol("q$i")) = Rotations.rotation_error(
-        R($(rot_state(R,P,i)...)), R($(rot_state(R,P,i,:x0)...)), errmap)) for i = 1:nr
+        R($(rot_state(R, P, i)...)), R($(rot_state(R, P, i, :x0)...)), errmap)) for i = 1:nr
     ]
 
     # Generate the vector of expressions for each element of the state differential
@@ -222,11 +222,11 @@ function _state_diff_expr(R,P)
     for i = 1:np
         # for the vector parts, simply subtract the indices from the original states
         if isodd(i)
-            vi = inds(R,P,i)
+            vi = inds(R, P, i)
             for j in vi
                 push!(dx, :(x[$j] - x0[$j]))
             end
-        # for the rotational parts, use the elements of the rotational errors generated above
+            # for the rotational parts, use the elements of the rotational errors generated above
         else
             r = i ÷ 2  # rotation index
             for j = 1:3
@@ -234,23 +234,23 @@ function _state_diff_expr(R,P)
             end
         end
     end
-    return dq,dx
+    return dq, dx
 end
-@generated function state_diff(s::LieState{R,P}, x::AbstractVector, x0::AbstractVector, 
-        errmap=Rotations.CayleyMap()) where {R,P}
+@generated function state_diff(s::LieState{R,P}, x::AbstractVector, x0::AbstractVector,
+    errmap=Rotations.CayleyMap()) where {R,P}
     nr = length(P) - 1   # number of rotations
-    n̄ = 3*nr + sum(P)    # error state size
-    dq,dx = _state_diff_expr(R,P)
+    n̄ = 3 * nr + sum(P)    # error state size
+    dq, dx = _state_diff_expr(R, P)
     quote
         $(Expr(:block, dq...))
         $(:(SVector{$n̄}(tuple($(dx...)))))
     end
 end
-@generated function state_diff!(s::LieState{R,P}, dx::AbstractVector, x::AbstractVector, 
-        x0::AbstractVector, errmap=Rotations.CayleyMap()) where {R,P}
-    dq,dx = _state_diff_expr(R,P)
-    dx_expr = map(enumerate(dx)) do (i,e)
-        :(dx[$i] = $e) 
+@generated function state_diff!(s::LieState{R,P}, dx::AbstractVector, x::AbstractVector,
+    x0::AbstractVector, errmap=Rotations.CayleyMap()) where {R,P}
+    dq, dx = _state_diff_expr(R, P)
+    dx_expr = map(enumerate(dx)) do (i, e)
+        :(dx[$i] = $e)
     end
     quote
         $(Expr(:block, dq...))
@@ -263,11 +263,11 @@ end
     nr = length(P) - 1   # number of rotations
     np = nr + length(P)  # number of partitions
     nv = length(P)
-    n̄ = 3*nr + sum(P)    # error state size
-    n = params(R)*nr + sum(P)
+    n̄ = 3 * nr + sum(P)    # error state size
+    n = params(R) * nr + sum(P)
 
     # Generate a vector of δq = q0\q expressions for each q in the state
-    q = [:(R($(rot_state(R,P,i)...))) for i = 1:nr]
+    q = [:(R($(rot_state(R, P, i)...))) for i = 1:nr]
 
     # Generate a vector of expressions assigning 1s to all the vector state diagonals
     Gv = Expr[]
@@ -275,7 +275,7 @@ end
     c = 1
     for k = 1:nv
         for j = 1:P[k]
-            push!(Gv, :(G[$(LinearIndices((n,n̄))[r,c])] = 1))
+            push!(Gv, :(G[$(LinearIndices((n, n̄))[r, c])] = 1))
             r += 1
             c += 1
         end
@@ -287,8 +287,8 @@ end
     Gr = Expr[]
     for k = 1:nr
         rinds = rot_inds(R, P, k)
-        cinds = (sum(P[1:k]) + 3*(k-1)) .+ (1:3)
-        push!(Gr, :(G[$rinds,$cinds] .= Rotations.∇differential($(q[k]))))
+        cinds = (sum(P[1:k]) + 3 * (k - 1)) .+ (1:3)
+        push!(Gr, :(G[$rinds, $cinds] .= Rotations.∇differential($(q[k]))))
     end
     quote
         $(Expr(:block, Gv...))
@@ -301,17 +301,17 @@ end
     nr = length(P) - 1   # number of rotations
     np = nr + length(P)  # number of partitions
     nv = length(P)
-    n̄ = 3*nr + sum(P)    # error state size
-    n = params(R)*nr + sum(P)
+    n̄ = 3 * nr + sum(P)    # error state size
+    n = params(R) * nr + sum(P)
 
     # Generate a vector of δq = q0\q expressions for each q in the state
-    q  = [:(R($(rot_state(R,P,i,:x)...)))  for i = 1:nr]
-    dq = [:(SVector{$(params(R))}($(rot_state(R,P,i,:dx)...))) for i = 1:nr]
+    q = [:(R($(rot_state(R, P, i, :x)...))) for i = 1:nr]
+    dq = [:(SVector{$(params(R))}($(rot_state(R, P, i, :dx)...))) for i = 1:nr]
 
     Gr = Expr[]
     for k = 1:nr
-        cinds = (sum(P[1:k]) + 3*(k-1)) .+ (1:3)
-        push!(Gr, :(view(∇G,$cinds,$cinds) .= SMatrix{3,3}(Rotations.∇²differential($(q[k]), $(dq[k])))))
+        cinds = (sum(P[1:k]) + 3 * (k - 1)) .+ (1:3)
+        push!(Gr, :(view(∇G, $cinds, $cinds) .= SMatrix{3,3}(Rotations.∇²differential($(q[k]), $(dq[k])))))
     end
     quote
         $(Expr(:block, Gr...))
diff --git a/src/rbstate.jl b/src/rbstate.jl
index 722af54..59058f1 100644
--- a/src/rbstate.jl
+++ b/src/rbstate.jl
@@ -28,16 +28,22 @@ An `RBState` can be converted to a state vector for a `RigidBody` using
 """
 struct RBState{T} <: StaticVector{13,T}
     r::SVector{3,T}
-    q::UnitQuaternion{T}
+    q::QuatRotation{T}
     v::SVector{3,T}
     ω::SVector{3,T}
-    function RBState{T}(r, q::Rotation, v, ω) where T
+    function RBState{T}(r, q::Rotation, v, ω) where {T}
         @assert length(r) == 3
         @assert length(v) == 3
         @assert length(ω) == 3
-        new{T}(r, UnitQuaternion{T}(q), v, ω)
+        new{T}(r, QuatRotation{T}(q), v, ω)
     end
-    @inline function RBState{T}(x::RBState) where T
+    function RBState{T}(r, q::QuatRotation, v, ω) where {T}
+        @assert length(r) == 3
+        @assert length(v) == 3
+        @assert length(ω) == 3
+        new{T}(r, QuatRotation{T}(q.q, false), v, ω)  # don't normalize
+    end
+    @inline function RBState{T}(x::RBState) where {T}
         RBState{T}(x.r, x.q, x.v, x.ω)
     end
 end
@@ -47,34 +53,39 @@ function RBState(r::AbstractVector, q::Rotation, v::AbstractVector, ω::Abstract
     RBState{T}(r, q, v, ω)
 end
 
+function RBState(r::AbstractVector, q::Quaternion{TQ}, v::AbstractVector, ω::AbstractVector) where {TQ}
+    T = promote_type(eltype(r), TQ, eltype(v), eltype(ω))
+    RBState{T}(r, QuatRotation(q, false), v, ω)
+end
+
 @inline function RBState(r::AbstractVector, q::AbstractVector, v::AbstractVector, ω::AbstractVector)
-    RBState(r, UnitQuaternion(q, false), v, ω)
+    RBState(r, QuatRotation(q, false), v, ω)
 end
 
 @inline RBState(x::RBState) = x
 
-@generated function RBState(::Type{R}, x::AbstractVector) where R<:Rotation{3}
-    ir = SA[1,2,3]
-    iq = SA[4,5,6]
-    iv = SA[7,8,9]
-    iω = SA[10,11,12]
-    if R <: UnitQuaternion
-        iq = SA[4,5,6,7]
+@generated function RBState(::Type{R}, x::AbstractVector) where {R<:Rotation{3}}
+    ir = SA[1, 2, 3]
+    iq = SA[4, 5, 6]
+    iv = SA[7, 8, 9]
+    iω = SA[10, 11, 12]
+    if R <: QuatRotation
+        iq = SA[4, 5, 6, 7]
         iv = iv .+ 1
         iω = iω .+ 1
     end
     quote
-        q = UnitQuaternion(R(x[$iq]))
+        q = QuatRotation(R(x[$iq]))
         RBState(x[$ir], q, x[$iv], x[$iω])
     end
 end
 
 
 # Static Arrays interface
-function (::Type{RB})(x::NTuple{13}) where RB <: RBState
+function (::Type{RB})(x::NTuple{13}) where {RB<:RBState}
     RB(
         SA[x[1], x[2], x[3]],
-        UnitQuaternion(x[4], x[5], x[6], x[7], false),
+        QuatRotation(x[4], x[5], x[6], x[7], false),
         SA[x[8], x[9], x[10]],
         SA[x[11], x[12], x[13]]
     )
@@ -102,7 +113,7 @@ Base.Tuple(x::RBState) = (
 
 Re-normalize the unit quaternion.
 """
-@inline renorm(x::RBState) = RBState(x.r, normalize(x.q), x.v, x.ω)
+@inline renorm(x::RBState) = RBState(x.r, normalize(x.q.q), x.v, x.ω)
 
 """
     position(x::RBState)
@@ -151,7 +162,7 @@ Add two rigid body states, which adds the position, linear and angular velocitie
     composes the orientations.
 """
 function Base.:+(s1::RBState, s2::RBState)
-    RBState(s1.r+s2.r, s1.q*s2.q, s1.v+s2.v, s1.ω+s2.ω)
+    RBState(s1.r + s2.r, s1.q * s2.q, s1.v + s2.v, s1.ω + s2.ω)
 end
 
 """
@@ -161,7 +172,7 @@ Substract two rigid body states, which substracts the position, linear and angul
     and composes the inverse of the second orientation with the first, i.e. `inv(q2)*q1`.
 """
 function Base.:-(s1::RBState, s2::RBState)
-    RBState(s1.r-s2.r, s2.q\s1.q, s1.v-s2.v, s1.ω-s2.ω)
+    RBState(s1.r - s2.r, s2.q \ s1.q, s1.v - s2.v, s1.ω - s2.ω)
 end
 
 """
@@ -171,12 +182,12 @@ Compute the 12-dimensional error state, calculated by substracting thep position
     and angular velocities, and using `Rotations.rotation_error` for the orientation.
 """
 function Rotations.:⊖(s1::RBState, s2::RBState, rmap=CayleyMap())
-    dx = s1.r-s2.r
+    dx = s1.r - s2.r
     dq = Rotations.rotation_error(s1.q, s2.q, rmap)
-    dv = s1.v-s2.v
-    dw = s1.ω-s2.ω
+    dv = s1.v - s2.v
+    dw = s1.ω - s2.ω
     @SVector [dx[1], dx[2], dx[3], dq[1], dq[2], dq[3],
-              dv[1], dv[2], dv[3], dw[1], dw[2], dw[3]]
+        dv[1], dv[2], dv[3], dw[1], dw[2], dw[3]]
 end
 
 """
@@ -186,9 +197,9 @@ Add the state error `dx` to the rigid body state `x` using the map `rmap`. Simpl
 vector states, and computes the orientation with `Rotations.add_error(x, dx, rmap)`
 """
 function Rotations.:⊕(s1::RBState, dx::StaticVector{12}, rmap=CayleyMap())
-    dr = SA[dx[1],  dx[2],  dx[3]]
-    dq = SA[dx[4],  dx[5],  dx[6]]
-    dv = SA[dx[7],  dx[8],  dx[9]]
+    dr = SA[dx[1], dx[2], dx[3]]
+    dq = SA[dx[4], dx[5], dx[6]]
+    dv = SA[dx[7], dx[8], dx[9]]
     dω = SA[dx[10], dx[11], dx[12]]
     q = Rotations.add_error(s1.q, Rotations.RotationError(dq, rmap))
     RBState(s1.r + dr, q, s1.v + dv, s1.ω + dω)
@@ -196,21 +207,21 @@ end
 
 Base.zero(s1::RBState) = zero(RBState)
 @inline Base.zero(::Type{<:RBState}) = zero(RBState{Float64})
-function Base.zero(::Type{<:RBState{T}}) where T
-    RBState((@SVector zeros(T,3)), UnitQuaternion{T}(I),
-        (@SVector zeros(T,3)), (@SVector zeros(T,3)))
+function Base.zero(::Type{<:RBState{T}}) where {T}
+    RBState((@SVector zeros(T, 3)), QuatRotation{T}(I),
+        (@SVector zeros(T, 3)), (@SVector zeros(T, 3)))
 end
 
 function Base.rand(::Type{RBState})
-    RBState(rand(3), rand(UnitQuaternion), rand(3), rand(3))
+    RBState(rand(3), rand(QuatRotation), rand(3), rand(3))
 end
 
 function randbetween(xmin::RBState, xmax::RBState)
     dx = xmax - xmin
     RBState(
         xmin.r .+ rand(3) .* dx.r,
-        # rand(UnitQuaternion),
-        expm((@SVector randn(3))*rand()*deg2rad(170)),
+        # rand(QuatRotation),
+        expm((@SVector randn(3)) * rand() * deg2rad(170)),
         xmin.v .+ rand(3) .* dx.v,
         xmin.ω .+ rand(3) .* dx.ω
     )
diff --git a/src/rigidbody.jl b/src/rigidbody.jl
index 9367f90..01861df 100644
--- a/src/rigidbody.jl
+++ b/src/rigidbody.jl
@@ -24,7 +24,7 @@ Instead of defining `forces` and `moments` you can define the higher-level `wren
 # Rotation Parameterization
 A `RigidBody` model must specify the rotational representation being used. Any `Rotations.Rotation{3}`
 can be used, but we suggest one of the following:
-* `UnitQuaternion`
+* `QuatRotation`
 * `MRP`
 * `RodriguesParam`
 
@@ -45,31 +45,31 @@ from the state vector representation for the given model.
 """
 abstract type RigidBody{R<:Rotation} <: LieGroupModel end
 
-LieState(::RigidBody{R}) where R = LieState(R, (3,6))
+LieState(::RigidBody{R}) where {R} = LieState(R, (3, 6))
 
 function Base.rand(model::RigidBody{D}) where {D}
-    n,m = dims(model)
+    n, m = dims(model)
     r = @SVector rand(3)
     q = rand(D)
     v = @SVector rand(3)
     ω = @SVector rand(3)
     x = build_state(model, r, q, v, ω)
     u = @SVector rand(m)  # NOTE: this is type unstable
-    return x,u
+    return x, u
 end
 
-function Base.zeros(model::RigidBody{D}) where D
-    n,m = dims(model)
+function Base.zeros(model::RigidBody{D}) where {D}
+    n, m = dims(model)
     r = @SVector zeros(3)
     q = one(D)
     v = @SVector zeros(3)
     ω = @SVector zeros(3)
     x = build_state(model, r, q, v, ω)
     u = @SVector zeros(m)  # NOTE: this is type unstable
-    return x,u
+    return x, u
 end
 
-@inline rotation_type(::RigidBody{D}) where D = D
+@inline rotation_type(::RigidBody{D}) where {D} = D
 
 """
     gen_inds(model::RigidBody)
@@ -77,11 +77,11 @@ end
 Generate a `NamedTuple` containing the indices of the position (`r`), orientation (`q`),
 linear velocity (`v`), and angular velocity (`ω`) from the state vector for `model`.
 """
-@generated function gen_inds(model::RigidBody{R}) where R
-    iF = SA[1,2,3]
-    iM = SA[4,5,6]
-    ir, iq, iv, iω = SA[1,2,3], SA[4,5,6], SA[7,8,9], SA[10,11,12]
-    if R <: UnitQuaternion
+@generated function gen_inds(model::RigidBody{R}) where {R}
+    iF = SA[1, 2, 3]
+    iM = SA[4, 5, 6]
+    ir, iq, iv, iω = SA[1, 2, 3], SA[4, 5, 6], SA[7, 8, 9], SA[10, 11, 12]
+    if R <: QuatRotation
         iq = push(iq, 7)
         iv = iv .+ 1
         iω = iω .+ 1
@@ -98,21 +98,21 @@ end
 @inline linear_velocity(model::RigidBody, x) = x[gen_inds(model).v]
 @inline angular_velocity(model::RigidBody, x) = x[gen_inds(model).ω]
 
-function orientation(model::RigidBody{<:UnitQuaternion}, x::AbstractVector,
-        renorm=false)
-    q = UnitQuaternion(x[4],x[5],x[6],x[7], renorm)
+function orientation(model::RigidBody{<:QuatRotation}, x::AbstractVector,
+    renorm=false)
+    q = QuatRotation(x[4], x[5], x[6], x[7], renorm)
     return q
 end
 for rot in [RodriguesParam, MRP, RotMatrix, RotationVec, AngleAxis]
-    @eval orientation(model::RigidBody{<:$rot}, x::AbstractVector, renorm=false) = ($rot)(x[4],x[5],x[6])
+    @eval orientation(model::RigidBody{<:$rot}, x::AbstractVector, renorm=false) = ($rot)(x[4], x[5], x[6])
 end
 
 """
     flipquat(model, x)
 
-Flips the quaternion sign for a `RigidBody{<:UnitQuaternion}`.
+Flips the quaternion sign for a `RigidBody{<:QuatRotation}`.
 """
-function flipquat(model::RigidBody{<:UnitQuaternion}, x)
+function flipquat(model::RigidBody{<:QuatRotation}, x)
     return @SVector [x[1], x[2], x[3], -x[4], -x[5], -x[6], -x[7],
         x[8], x[9], x[10], x[11], x[12], x[13]]
 end
@@ -121,7 +121,7 @@ end
     parse_state(model::RigidBody{R}, x, renorm=false)
 
 Return the position, orientation, linear velocity, and angular velocity as separate vectors.
-The orientation will be of type `R`. If `renorm=true` and `R <: UnitQuaternion` the quaternion
+The orientation will be of type `R`. If `renorm=true` and `R <: QuatRotation` the quaternion
 will be renormalized.
 """
 function parse_state(model::RigidBody, x, renorm=false)
@@ -152,14 +152,14 @@ end
     build_state(model::RigidBody{R}, x::AbstractVector) where R
     build_state(model::RigidBody{R}, r, q, v, ω) where R
 
-Build the state vector for `model` using the `RBState` `x`. If `R <: UnitQuaternion` this
+Build the state vector for `model` using the `RBState` `x`. If `R <: QuatRotation` this
     returns `x` cast as an `SVector`, otherwise it will convert the quaternion in `x` to
     a rotation of type `R`.
 
 Also accepts as arguments any arguments that can be passed to the constructor of `RBState`.
 """
-@inline build_state(model::RigidBody{<:UnitQuaternion}, x::RBState) = SVector(x)
-function build_state(model::RigidBody{R}, x::RBState) where R <: Rotation
+@inline build_state(model::RigidBody{<:QuatRotation}, x::RBState) = SVector(x)
+function build_state(model::RigidBody{R}, x::RBState) where {R<:Rotation}
     r = position(x)
     q = Rotations.params(R(orientation(x)))
     v = linear_velocity(x)
@@ -172,13 +172,13 @@ function build_state(model::RigidBody{R}, x::RBState) where R <: Rotation
     ]
 end
 
-function build_state(model::RigidBody{R}, args...) where R <: Rotation
+function build_state(model::RigidBody{R}, args...) where {R<:Rotation}
     x_ = RBState(args...)
     build_state(model, x_)
 end
 
 @inline function build_state(model::RigidBody,
-        r::AbstractVector, q::AbstractVector, v::AbstractVector, ω::AbstractVector)
+    r::AbstractVector, q::AbstractVector, v::AbstractVector, ω::AbstractVector)
     @assert length(q) == 3
     SA[
         r[1], r[2], r[3],
@@ -188,8 +188,8 @@ end
     ]
 end
 
-@inline function build_state(model::RigidBody{<:UnitQuaternion},
-        r::AbstractVector, q::AbstractVector, v::AbstractVector, ω::AbstractVector)
+@inline function build_state(model::RigidBody{<:QuatRotation},
+    r::AbstractVector, q::AbstractVector, v::AbstractVector, ω::AbstractVector)
     @assert length(q) == 4
     SA[
         r[1], r[2], r[3],
@@ -199,12 +199,12 @@ end
     ]
 end
 
-function fill_state(model::RigidBody{<:UnitQuaternion}, x::Real, q::Real, v::Real, ω::Real)
-    @SVector [x,x,x, q,q,q,q, v,v,v, ω,ω,ω]
+function fill_state(model::RigidBody{<:QuatRotation}, x::Real, q::Real, v::Real, ω::Real)
+    @SVector [x, x, x, q, q, q, q, v, v, v, ω, ω, ω]
 end
 
 function fill_state(model::RigidBody, x::Real, q::Real, v::Real, ω::Real)
-    @SVector [x,x,x, q,q,q, v,v,v, ω,ω,ω]
+    @SVector [x, x, x, q, q, q, v, v, v, ω, ω, ω]
 end
 
 ############################################################################################
@@ -212,7 +212,7 @@ end
 ############################################################################################
 function dynamics(model::RigidBody, x, u, t=0)
 
-    r,q,v,ω = parse_state(model, x)
+    r, q, v, ω = parse_state(model, x)
 
     ξ = wrenches(model, x, u, t)
     F = SA[ξ[1], ξ[2], ξ[3]]  # forces in world frame
@@ -221,22 +221,22 @@ function dynamics(model::RigidBody, x, u, t=0)
     J = inertia(model)
     Jinv = inertia_inv(model)
 
-    qdot = Rotations.kinematics(q,ω)
+    qdot = Rotations.kinematics(q, ω)
     if velocity_frame(model) == :world
         xdot = v
         vdot = F ./ m
     elseif velocity_frame(model) == :body
         xdot = q * v
-        vdot = q\(F ./ m) - ω × v
+        vdot = q \ (F ./ m) - ω × v
     end
-    ωdot = Jinv*(τ - ω × (J*ω))
+    ωdot = Jinv * (τ - ω × (J * ω))
 
     build_state(model, xdot, qdot, vdot, ωdot)
     # [xdot; qdot; vdot; ωdot]
 end
 
 # Use the StaticArrays methods since we know the size will always be small enough
-@inline function dynamics!(model::RigidBody{D}, xdot, x, u, t=0) where D
+@inline function dynamics!(model::RigidBody{D}, xdot, x, u, t=0) where {D}
     xdot .= dynamics(model, x, u, t)
 end
 
@@ -257,13 +257,13 @@ end
 @inline moments(::RigidBody, x, u)::SVector{3} = throw(ErrorException("Not implemented"))
 @inline velocity_frame(::RigidBody) = :world # :body or :world
 
-function jacobian!(model::RigidBody{<:UnitQuaternion}, F, y, x, u, t)
-    iF = SA[1,2,3]
-    iM = SA[4,5,6]
-    ir,iq,iv,iω,iu = gen_inds(model)
+function jacobian!(model::RigidBody{<:QuatRotation}, F, y, x, u, t)
+    iF = SA[1, 2, 3]
+    iM = SA[4, 5, 6]
+    ir, iq, iv, iω, iu = gen_inds(model)
 
     # Extract the info from the state and model
-    r,q,v,ω = parse_state(model, x)
+    r, q, v, ω = parse_state(model, x)
     m = mass(model)
     J = inertia(model)
     Jinv = inertia_inv(model)
@@ -278,35 +278,35 @@ function jacobian!(model::RigidBody{<:UnitQuaternion}, F, y, x, u, t)
     wrench_jacobian!(Jw, model, z)
     js = wrench_sparsity(model)
     if velocity_frame(model) == :world
-        tmp = I * 1/m
+        tmp = I * 1 / m
     else
-        tmp = 1/m * RotMatrix(inv(q))
+        tmp = 1 / m * RotMatrix(inv(q))
     end
-    js[1,1] && (Jw[iF, ir] .= tmp * Jw[iF, ir])
-    js[1,2] && (Jw[iF, iq] .= tmp * Jw[iF, iq])
-    js[1,3] && (Jw[iF, iv] .= tmp * Jw[iF, iv])
-    js[1,4] && (Jw[iF, iω] .= tmp * Jw[iF, iω])
-    js[1,5] && (Jw[iF, iu] .= tmp * Jw[iF, iu])
-    js[2,1] && (Jw[iM, ir] .= Jinv * Jw[iM, ir])
-    js[2,2] && (Jw[iM, iq] .= Jinv * Jw[iM, iq])
-    js[2,3] && (Jw[iM, iv] .= Jinv * Jw[iM, iv])
-    js[2,4] && (Jw[iM, iω] .= Jinv * Jw[iM, iω])
-    js[2,5] && (Jw[iM, iu] .= Jinv * Jw[iM, iu])
+    js[1, 1] && (Jw[iF, ir] .= tmp * Jw[iF, ir])
+    js[1, 2] && (Jw[iF, iq] .= tmp * Jw[iF, iq])
+    js[1, 3] && (Jw[iF, iv] .= tmp * Jw[iF, iv])
+    js[1, 4] && (Jw[iF, iω] .= tmp * Jw[iF, iω])
+    js[1, 5] && (Jw[iF, iu] .= tmp * Jw[iF, iu])
+    js[2, 1] && (Jw[iM, ir] .= Jinv * Jw[iM, ir])
+    js[2, 2] && (Jw[iM, iq] .= Jinv * Jw[iM, iq])
+    js[2, 3] && (Jw[iM, iv] .= Jinv * Jw[iM, iv])
+    js[2, 4] && (Jw[iM, iω] .= Jinv * Jw[iM, iω])
+    js[2, 5] && (Jw[iM, iu] .= Jinv * Jw[iM, iu])
 
     # Add in the parts of the Jacobian that are not functions of the wrench
     F[iq, iq] .= 0.5 * Rotations.rmult(Rotations.pure_quaternion(ω))
     F[iq, iω] .= 0.5 * Rotations.lmult(q) * Rotations.hmat()
-    F[iω, iω] .= Jinv*(skew(J*ω) - skew(ω)*J) + F[iω,iω]
+    F[iω, iω] .= Jinv * (skew(J * ω) - skew(ω) * J) + F[iω, iω]
     if velocity_frame(model) == :world
-        F[1,8] += 1
-        F[2,9] += 1
-        F[3,10] += 1
+        F[1, 8] += 1
+        F[2, 9] += 1
+        F[3, 10] += 1
     else
-        F[ir,iq] .+= Rotations.∇rotate(q, v)
-        F[ir,iv] .+= RotMatrix(q)
-        F[iv,iq] .+= 1/m * Rotations.∇rotate(inv(q), f) * Rotations.tmat()
-        F[iv,iv] .+= -Rotations.skew(ω)
-        F[iv,iω] .+=  Rotations.skew(v)
+        F[ir, iq] .+= Rotations.∇rotate(q, v)
+        F[ir, iv] .+= RotMatrix(q)
+        F[iv, iq] .+= 1 / m * Rotations.∇rotate(inv(q), f) * Rotations.tmat()
+        F[iv, iv] .+= -Rotations.skew(ω)
+        F[iv, iω] .+= Rotations.skew(v)
     end
 
     return F
@@ -344,4 +344,4 @@ SA[false true  false false true;
    false false false false true]
 ```
 """
-wrench_sparsity(model::RigidBody) = @SMatrix ones(Bool,2,5)
+wrench_sparsity(model::RigidBody) = @SMatrix ones(Bool, 2, 5)
diff --git a/test/implicit_dynamics_test.jl b/test/implicit_dynamics_test.jl
index 6001fab..446341f 100644
--- a/test/implicit_dynamics_test.jl
+++ b/test/implicit_dynamics_test.jl
@@ -11,150 +11,153 @@ const RD = RobotDynamics
 include("cartpole_model.jl")
 
 ## Test the custom LU method
-function testcustomlu()
-    n,m = 4,2
-    J1 = randn(n, n+m)
-    A = @view J1[:,1:n]
+function testcustomlu(runtest=true)
+    n, m = 4, 2
+    J1 = randn(n, n + m)
+    A = @view J1[:, 1:n]
     A2 = copy(A)
 
-    ipiv = similar(A, BLAS.BlasInt, min(n, n+m))
+    ipiv = similar(A, BLAS.BlasInt, min(n, n + m))
     # @allocated LAPACK.getrf!(A, ipiv)
     allocs = @allocated F = lu!(A, ipiv)
     F2 = lu!(A)
-    @test F2.L ≈ F2.L
-    @test F2.U ≈ F2.U
-    @test allocs == 0
+    if runtest
+        @test F2.L ≈ F2.L
+        @test F2.U ≈ F2.U
+        @test allocs == 0
+    end
 end
+testcustomlu(false)  # run once to compile
 testcustomlu()
 
 ##
 @testset "Implicit to Explicit" begin
-model = Cartpole()
-integrator = RD.ImplicitMidpoint(model)
-dmodel = RD.DiscretizedDynamics(model, integrator)
-dmodel_rk4 = RD.DiscretizedDynamics(model, RD.RK4(model))
-n,m = RD.dims(model)
-x1 = zeros(n)
-u1 = fill(1.0, m)
-dt = 0.01
-z1 = KnotPoint{n,m}(x1, u1, 0.0, dt)
-x2_rk4 = RD.discrete_dynamics(dmodel_rk4, z1)
-
-function midpoint_residual(x2, z1)
-    n,m = RD.dims(z1)
-    v = zeros(eltype(x2), n+m)
-    z2 = RD.StaticKnotPoint(z1, v)
-    RD.setstate!(z2, x2)
-    RD.dynamics_error(dmodel, z2, z1)
-end
+    model = Cartpole()
+    integrator = RD.ImplicitMidpoint(model)
+    dmodel = RD.DiscretizedDynamics(model, integrator)
+    dmodel_rk4 = RD.DiscretizedDynamics(model, RD.RK4(model))
+    n, m = RD.dims(model)
+    x1 = zeros(n)
+    u1 = fill(1.0, m)
+    dt = 0.01
+    z1 = KnotPoint{n,m}(x1, u1, 0.0, dt)
+    x2_rk4 = RD.discrete_dynamics(dmodel_rk4, z1)
+
+    function midpoint_residual(x2, z1)
+        n, m = RD.dims(z1)
+        v = zeros(eltype(x2), n + m)
+        z2 = RD.StaticKnotPoint(z1, v)
+        RD.setstate!(z2, x2)
+        RD.dynamics_error(dmodel, z2, z1)
+    end
 
-function newton_solve(v)
-    z = RD.StaticKnotPoint(z1, v)
-    n,m = RD.dims(z)
-    r(xn) = midpoint_residual(xn, z)
+    function newton_solve(v)
+        z = RD.StaticKnotPoint(z1, v)
+        n, m = RD.dims(z)
+        r(xn) = midpoint_residual(xn, z)
 
-    xn = copy(RD.state(z))
-    for i = 1:10
-        res = r(xn)
-        if norm(res) < 1e-10
-            break
-        end
+        xn = copy(RD.state(z))
+        for i = 1:10
+            res = r(xn)
+            if norm(res) < 1e-10
+                break
+            end
 
-        ∇r = ForwardDiff.jacobian(r, xn)
-        xn .-= ∇r\res
+            ∇r = ForwardDiff.jacobian(r, xn)
+            xn .-= ∇r \ res
+        end
+        return xn
     end
-    return xn
-end
-x2 = newton_solve(RD.getdata(z1))
-@test norm(midpoint_residual(x2, z1)) < √eps() 
-
-# Test in-place evaluation
-x2_inplace = copy(x1)
-RD.discrete_dynamics!(dmodel, x2_inplace, z1)
-@test norm(midpoint_residual(x2_inplace, z1)) < √eps() 
-x2_inplace .= 0
-RD.evaluate!(dmodel, x2_inplace, z1)
-@test norm(midpoint_residual(x2_inplace, z1)) < √eps() 
-
-
-# Test static evaluation
-x1 = [1,pi,1.3,1.1]
-z1 = KnotPoint{n,m}(x1, u1, 0.0, dt)
-z1s = KnotPoint(SVector{n}(x1), SVector{m}(u1), z1.t, z1.dt)
-x2_static = RD.discrete_dynamics(dmodel, z1s)
-@test norm(midpoint_residual(x2_static, z1s)) < √eps() 
-x2_static = RD.evaluate(dmodel, z1s)
-@test norm(midpoint_residual(x2_static, z1s)) < √eps() 
-
-# Test Jacobian
-J = zeros(n, n+m)
-y = zeros(n)
-diff = RD.default_diffmethod(dmodel)
-@test diff isa RD.ImplicitFunctionTheorem
-@test diff === RD.ImplicitFunctionTheorem(RD.UserDefined())
-
-using Random
-Random.seed!(1)
-for i = 1:4
-    x = randn(n)
-    u = randn(m)
-    z1 = KnotPoint{n,m}(x, u, 0.0, dt)
-    Jfd = FiniteDiff.finite_difference_jacobian(newton_solve, RD.getdata(z1))
-
-    if iseven(i)
-        RD.discrete_dynamics!(dmodel, y, z1)
+    x2 = newton_solve(RD.getdata(z1))
+    @test norm(midpoint_residual(x2, z1)) < √eps()
+
+    # Test in-place evaluation
+    x2_inplace = copy(x1)
+    RD.discrete_dynamics!(dmodel, x2_inplace, z1)
+    @test norm(midpoint_residual(x2_inplace, z1)) < √eps()
+    x2_inplace .= 0
+    RD.evaluate!(dmodel, x2_inplace, z1)
+    @test norm(midpoint_residual(x2_inplace, z1)) < √eps()
+
+
+    # Test static evaluation
+    x1 = [1, pi, 1.3, 1.1]
+    z1 = KnotPoint{n,m}(x1, u1, 0.0, dt)
+    z1s = KnotPoint(SVector{n}(x1), SVector{m}(u1), z1.t, z1.dt)
+    x2_static = RD.discrete_dynamics(dmodel, z1s)
+    @test norm(midpoint_residual(x2_static, z1s)) < √eps()
+    x2_static = RD.evaluate(dmodel, z1s)
+    @test norm(midpoint_residual(x2_static, z1s)) < √eps()
+
+    # Test Jacobian
+    J = zeros(n, n + m)
+    y = zeros(n)
+    diff = RD.default_diffmethod(dmodel)
+    @test diff isa RD.ImplicitFunctionTheorem
+    @test diff === RD.ImplicitFunctionTheorem(RD.UserDefined())
+
+    using Random
+    Random.seed!(1)
+    for i = 1:4
+        x = randn(n)
+        u = randn(m)
+        z1 = KnotPoint{n,m}(x, u, 0.0, dt)
+        Jfd = FiniteDiff.finite_difference_jacobian(newton_solve, RD.getdata(z1))
+
+        if iseven(i)
+            RD.discrete_dynamics!(dmodel, y, z1)
+        end
+        RD.jacobian!(RD.InPlace(), diff, dmodel, J, y, z1)
+        @test norm(Jfd - J) < 1e-4
     end
-    RD.jacobian!(RD.InPlace(), diff, dmodel, J, y, z1)
-    @test norm(Jfd - J) < 1e-4
-end
 
-for i = 1:4
-    x = randn(n)
-    u = randn(m)
-    z1 = KnotPoint{n,m}(x, u, 0.0, dt)
-    z1s = KnotPoint(SVector{n}(x), SVector{m}(u), z1.t, z1.dt)
-    Jfd = FiniteDiff.finite_difference_jacobian(newton_solve, RD.getdata(z1))
+    for i = 1:4
+        x = randn(n)
+        u = randn(m)
+        z1 = KnotPoint{n,m}(x, u, 0.0, dt)
+        z1s = KnotPoint(SVector{n}(x), SVector{m}(u), z1.t, z1.dt)
+        Jfd = FiniteDiff.finite_difference_jacobian(newton_solve, RD.getdata(z1))
 
-    if iseven(i)
-        RD.discrete_dynamics(dmodel, z1s)
+        if iseven(i)
+            RD.discrete_dynamics(dmodel, z1s)
+        end
+        RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, y, z1s)
+        @test norm(Jfd - J) < 1e-4
     end
-    RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, y, z1s)
-    @test norm(Jfd - J) < 1e-4
-end
 
-@test_throws ArgumentError RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dmodel, J, y, z1)
-@test_throws ArgumentError RD.jacobian!(RD.StaticReturn(), RD.ForwardAD(), dmodel, J, y, z1)
-@test_throws ArgumentError RD.jacobian!(RD.StaticReturn(), RD.UserDefined(), dmodel, J, y, z1)
-@test_throws ArgumentError RD.jacobian!(RD.InPlace(), RD.FiniteDifference(), dmodel, J, y, z1)
-
-ENV["JULIA_DEBUG"] = "RobotDynamics"
-z1.z .+= 1
-@test_logs (:debug, r"Solving for next") RD.jacobian!(RD.InPlace(), diff, dmodel, J, y, z1)
-@test_logs (:debug, r"Using cached") RD.jacobian!(RD.InPlace(), diff, dmodel, J, y, z1)
-RD.setdata!(z1s, z1.z .+ 1)
-@test_logs (:debug, r"Solving for next") RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, y, z1s)
-@test_logs (:debug, r"Using cached") RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, y, z1s)
-
-ENV["JULIA_DEBUG"] = ""
-function test_implicit_allocs(dmodel, z::RD.AbstractKnotPoint{Nx,Nu}) where {Nx,Nu}
-    zs = KnotPoint{Nx,Nu}(SVector{Nx+Nu}(RD.getdata(z)), z.t, z.dt)
-    x2 = zeros(RD.state_dim(dmodel))
-    J = zeros(Nx, Nx + Nu)
-    y = zeros(Nx)
-    diffmethod = RD.default_diffmethod(dmodel)
-    allocs = @allocated RD.discrete_dynamics!(dmodel, x2, z)
-    allocs += @allocated RD.jacobian!(RD.InPlace(), diffmethod, dmodel, J, y, z)
-
-    allocs += @allocated RD.discrete_dynamics(dmodel, zs)
-    allocs += @allocated RD.jacobian!(RD.StaticReturn(), diffmethod, dmodel, J, y, zs)
-
-    z[1] += 1
-    allocs += @allocated RD.jacobian!(RD.InPlace(), diffmethod, dmodel, J, y, z)
-
-    zs.z = zs.z .+ 0.2
-    allocs += @allocated RD.jacobian!(RD.StaticReturn(), diffmethod, dmodel, J, y, zs)
-    return allocs
-end
-test_implicit_allocs(dmodel, z1)  # run once to compile non-logging versions
-run_alloc_tests && @test test_implicit_allocs(dmodel, z1) == 0
+    @test_throws ArgumentError RD.jacobian!(RD.InPlace(), RD.ForwardAD(), dmodel, J, y, z1)
+    @test_throws ArgumentError RD.jacobian!(RD.StaticReturn(), RD.ForwardAD(), dmodel, J, y, z1)
+    @test_throws ArgumentError RD.jacobian!(RD.StaticReturn(), RD.UserDefined(), dmodel, J, y, z1)
+    @test_throws ArgumentError RD.jacobian!(RD.InPlace(), RD.FiniteDifference(), dmodel, J, y, z1)
+
+    ENV["JULIA_DEBUG"] = "RobotDynamics"
+    z1.z .+= 1
+    @test_logs (:debug, r"Solving for next") RD.jacobian!(RD.InPlace(), diff, dmodel, J, y, z1)
+    @test_logs (:debug, r"Using cached") RD.jacobian!(RD.InPlace(), diff, dmodel, J, y, z1)
+    RD.setdata!(z1s, z1.z .+ 1)
+    @test_logs (:debug, r"Solving for next") RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, y, z1s)
+    @test_logs (:debug, r"Using cached") RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, y, z1s)
+
+    ENV["JULIA_DEBUG"] = ""
+    function test_implicit_allocs(dmodel, z::RD.AbstractKnotPoint{Nx,Nu}) where {Nx,Nu}
+        zs = KnotPoint{Nx,Nu}(SVector{Nx + Nu}(RD.getdata(z)), z.t, z.dt)
+        x2 = zeros(RD.state_dim(dmodel))
+        J = zeros(Nx, Nx + Nu)
+        y = zeros(Nx)
+        diffmethod = RD.default_diffmethod(dmodel)
+        allocs = @allocated RD.discrete_dynamics!(dmodel, x2, z)
+        allocs += @allocated RD.jacobian!(RD.InPlace(), diffmethod, dmodel, J, y, z)
+
+        allocs += @allocated RD.discrete_dynamics(dmodel, zs)
+        allocs += @allocated RD.jacobian!(RD.StaticReturn(), diffmethod, dmodel, J, y, zs)
+
+        z[1] += 1
+        allocs += @allocated RD.jacobian!(RD.InPlace(), diffmethod, dmodel, J, y, z)
+
+        zs.z = zs.z .+ 0.2
+        allocs += @allocated RD.jacobian!(RD.StaticReturn(), diffmethod, dmodel, J, y, zs)
+        return allocs
+    end
+    test_implicit_allocs(dmodel, z1)  # run once to compile non-logging versions
+    run_alloc_tests && @test test_implicit_allocs(dmodel, z1) == 0
 end
\ No newline at end of file
diff --git a/test/liemodel.jl b/test/liemodel.jl
index 5a51f5f..7a8dc91 100644
--- a/test/liemodel.jl
+++ b/test/liemodel.jl
@@ -6,17 +6,17 @@ Satellite() = Satellite(Diagonal(@SVector ones(3)))
 
 RobotDynamics.control_dim(::Satellite) = 3
 Base.position(::Satellite, x::SVector) = @SVector zeros(3)
-RobotDynamics.orientation(::Satellite, x::SVector) = UnitQuaternion(x[4], x[5], x[6], x[7])
+RobotDynamics.orientation(::Satellite, x::SVector) = QuatRotation(x[4], x[5], x[6], x[7])
 
-RobotDynamics.LieState(::Satellite) = RobotDynamics.LieState(UnitQuaternion{Float64}, (3,0))
+RobotDynamics.LieState(::Satellite) = RobotDynamics.LieState(QuatRotation{Float64}, (3, 0))
 
 function RobotDynamics.dynamics(model::Satellite, x::SVector, u::SVector)
     ω = @SVector [x[1], x[2], x[3]]
     q = normalize(@SVector [x[4], x[5], x[6], x[7]])
     J = model.J
 
-    ωdot = J\(u - ω × (J*ω))
-    qdot = 0.5*lmult(q)*hmat()*ω
+    ωdot = J \ (u - ω × (J * ω))
+    qdot = 0.5 * lmult(q) * hmat() * ω
     return [ωdot; qdot]
 end
 
@@ -25,16 +25,16 @@ model = Satellite()
 @test RD.state_dim(model) == 7
 @test RD.errstate_dim(model) == 6
 
-x,u = rand(model)
+x, u = rand(model)
 s = LieState(model)
-@test all(RobotDynamics.vec_states(s, x) .≈ (x[1:3],Float64[]))
-@test all(RobotDynamics.rot_states(s, x) .≈ (UnitQuaternion(x[4:7]),))
-@test all(RobotDynamics.vec_states(model, x) .≈ (x[1:3],Float64[]))
-@test all(RobotDynamics.rot_states(model, x) .≈ (UnitQuaternion(x[4:7]),))
+@test all(RobotDynamics.vec_states(s, x) .≈ (x[1:3], Float64[]))
+@test all(RobotDynamics.rot_states(s, x) .≈ (QuatRotation(x[4:7]),))
+@test all(RobotDynamics.vec_states(model, x) .≈ (x[1:3], Float64[]))
+@test all(RobotDynamics.rot_states(model, x) .≈ (QuatRotation(x[4:7]),))
 
 x2 = rand(s)
 @test norm(x[4:7]) ≈ 1
 
 dx = RobotDynamics.state_diff(model, x, x2)
 @test length(dx) == 6
-@test dx ≈ [x[1:3] - x2[1:3]; UnitQuaternion(x[4:7]) ⊖ UnitQuaternion(x2[4:7])]
+@test dx ≈ [x[1:3] - x2[1:3]; QuatRotation(x[4:7]) ⊖ QuatRotation(x2[4:7])]
diff --git a/test/liestate.jl b/test/liestate.jl
index a9b5e48..687475b 100644
--- a/test/liestate.jl
+++ b/test/liestate.jl
@@ -9,98 +9,98 @@ import RobotDynamics: LieState
 const RD = RobotDynamics
 
 function error_state_quat(x::AbstractVector, x0::AbstractVector)
-    dq1 = UnitQuaternion(x[4],x[5],x[6],x[7]) ⊖ UnitQuaternion(x0[4],x0[5],x0[6],x0[7])
-    dq2 = UnitQuaternion(x[10],x[11],x[12],x[13]) ⊖ UnitQuaternion(x0[10],x0[11],x0[12],x0[13])
+    dq1 = QuatRotation(x[4], x[5], x[6], x[7]) ⊖ QuatRotation(x0[4], x0[5], x0[6], x0[7])
+    dq2 = QuatRotation(x[10], x[11], x[12], x[13]) ⊖ QuatRotation(x0[10], x0[11], x0[12], x0[13])
     dx = x - x0
     SA[dx[1], dx[2], dx[3], dq1[1], dq1[2], dq1[3], dx[8], dx[9], dq2[1], dq2[2], dq2[3], dx[14], dx[15], dx[16]]
 end
 
 function error_state_rp(x::AbstractVector, x0::AbstractVector)
-    dq1 = RodriguesParam(x[4],x[5],x[6]) ⊖ RodriguesParam(x0[4],x0[5],x0[6])
-    dq2 = RodriguesParam(x[9],x[10],x[11]) ⊖ RodriguesParam(x0[9],x0[10],x0[11])
+    dq1 = RodriguesParam(x[4], x[5], x[6]) ⊖ RodriguesParam(x0[4], x0[5], x0[6])
+    dq2 = RodriguesParam(x[9], x[10], x[11]) ⊖ RodriguesParam(x0[9], x0[10], x0[11])
     dx = x - x0
     SA[dx[1], dx[2], dx[3], dq1[1], dq1[2], dq1[3],
-       dx[7], dx[8],        dq2[1], dq2[2], dq2[3], dx[12], dx[13], dx[14]]
+        dx[7], dx[8], dq2[1], dq2[2], dq2[3], dx[12], dx[13], dx[14]]
 end
 
 # Test params
-@test Rotations.params(UnitQuaternion) == 4
+@test Rotations.params(QuatRotation) == 4
 @test Rotations.params(RodriguesParam) == 3
 @test Rotations.params(MRP) == 3
 @test Rotations.params(RotMatrix3) == 9
 @test Rotations.params(RotMatrix2) == 4
 
 # Test index functions
-R = UnitQuaternion{Float64}
-P = (3,2,3)
+R = QuatRotation{Float64}
+P = (3, 2, 3)
 s = LieState{R,P}()
 @test length(s) == 16
 @test RD.errstate_dim(s) == 14
-@test RobotDynamics.vec_inds(R,P,1) == 1:3   == RobotDynamics.inds(R,P,1)
-@test RobotDynamics.rot_inds(R,P,1) == 4:7   == RobotDynamics.inds(R,P,2)
-@test RobotDynamics.vec_inds(R,P,2) == 8:9   == RobotDynamics.inds(R,P,3)
-@test RobotDynamics.rot_inds(R,P,2) == 10:13 == RobotDynamics.inds(R,P,4)
-@test RobotDynamics.vec_inds(R,P,3) == 14:16 == RobotDynamics.inds(R,P,5)
-@test RobotDynamics.QuatState(16, SA[4,10]) === s
-@test RobotDynamics.QuatState(16, @MVector [4,10]) === s
-@test RobotDynamics.QuatState(16, (4,10)) === s
+@test RobotDynamics.vec_inds(R, P, 1) == 1:3 == RobotDynamics.inds(R, P, 1)
+@test RobotDynamics.rot_inds(R, P, 1) == 4:7 == RobotDynamics.inds(R, P, 2)
+@test RobotDynamics.vec_inds(R, P, 2) == 8:9 == RobotDynamics.inds(R, P, 3)
+@test RobotDynamics.rot_inds(R, P, 2) == 10:13 == RobotDynamics.inds(R, P, 4)
+@test RobotDynamics.vec_inds(R, P, 3) == 14:16 == RobotDynamics.inds(R, P, 5)
+@test RobotDynamics.QuatState(16, SA[4, 10]) === s
+@test RobotDynamics.QuatState(16, @MVector [4, 10]) === s
+@test RobotDynamics.QuatState(16, (4, 10)) === s
 @test RobotDynamics.num_rotations(s) == 2
 @test length(typeof(s)) == length(s)
 
 n = length(s)
-x  = @MVector rand(n)
+x = @MVector rand(n)
 x0 = @MVector rand(n)
 @test RobotDynamics.state_diff(s, x, x0) ≈ error_state_quat(x, x0)
 
 # Test with RP
 R = RodriguesParam{Float64}
-P = (3,2,3)
+P = (3, 2, 3)
 s = LieState{R,P}()
-@test LieState(R,P) === s
-@test LieState(R,3,2,3) === s
+@test LieState(R, P) === s
+@test LieState(R, 3, 2, 3) === s
 @test length(s) == 14
 @test RD.errstate_dim(s) == 14
-@test RobotDynamics.vec_inds(R,P,1) == 1:3   == RobotDynamics.inds(R,P,1)
-@test RobotDynamics.rot_inds(R,P,1) == 4:6   == RobotDynamics.inds(R,P,2)
-@test RobotDynamics.vec_inds(R,P,2) == 7:8   == RobotDynamics.inds(R,P,3)
-@test RobotDynamics.rot_inds(R,P,2) == 9:11  == RobotDynamics.inds(R,P,4)
-@test RobotDynamics.vec_inds(R,P,3) == 12:14 == RobotDynamics.inds(R,P,5)
+@test RobotDynamics.vec_inds(R, P, 1) == 1:3 == RobotDynamics.inds(R, P, 1)
+@test RobotDynamics.rot_inds(R, P, 1) == 4:6 == RobotDynamics.inds(R, P, 2)
+@test RobotDynamics.vec_inds(R, P, 2) == 7:8 == RobotDynamics.inds(R, P, 3)
+@test RobotDynamics.rot_inds(R, P, 2) == 9:11 == RobotDynamics.inds(R, P, 4)
+@test RobotDynamics.vec_inds(R, P, 3) == 12:14 == RobotDynamics.inds(R, P, 5)
 @test RobotDynamics.state_diff(s, x, x0) ≈ error_state_rp(x, x0)
 
 # @btime error_state($s, $x, $x0)
 # @btime error_state_rp($x, $x0)
 
 # state diff jacobian
-R = UnitQuaternion{Float64}
-P = (3,2,3)
+R = QuatRotation{Float64}
+P = (3, 2, 3)
 s = LieState{R,P}()
 n = length(s)
 n̄ = RD.errstate_dim(s)
-G = zeros(n,n̄)
+G = zeros(n, n̄)
 RobotDynamics.errstate_jacobian!(s, G, x)
 
-q1 = UnitQuaternion(x[4],x[5],x[6],x[7])
-q2 = UnitQuaternion(x[10],x[11],x[12],x[13])
+q1 = QuatRotation(x[4], x[5], x[6], x[7])
+q2 = QuatRotation(x[10], x[11], x[12], x[13])
 G1 = Rotations.∇differential(q1)
 G2 = Rotations.∇differential(q2)
-G0 = cat(I(3),G1,I(2),G2,I(3),dims=(1,2))
+G0 = cat(I(3), G1, I(2), G2, I(3), dims=(1, 2))
 @test G0 ≈ G
 
 # state diff jacobian jacobian
-∇G = SizedMatrix{n̄,n̄}(zeros(n̄,n̄))
+∇G = SizedMatrix{n̄,n̄}(zeros(n̄, n̄))
 dx = @SVector rand(n)
 
-dq1 = SVector(dx[4],dx[5],dx[6],dx[7])
-dq2 = SVector(dx[10],dx[11],dx[12],dx[13])
+dq1 = SVector(dx[4], dx[5], dx[6], dx[7])
+dq2 = SVector(dx[10], dx[11], dx[12], dx[13])
 RobotDynamics.∇errstate_jacobian!(s, ∇G, x, dx)
 ∇G1 = Rotations.∇²differential(q1, dq1)
 ∇G2 = Rotations.∇²differential(q2, dq2)
-∇G0 = cat(zeros(3,3), ∇G1, zeros(2,2), ∇G2, zeros(3,3), dims=(1,2))
+∇G0 = cat(zeros(3, 3), ∇G1, zeros(2, 2), ∇G2, zeros(3, 3), dims=(1, 2))
 @test Matrix(∇G0) ≈ Matrix(∇G)
 
 struct LieBody <: RD.LieGroupModel end
 RobotDynamics.control_dim(::LieBody) = 4
-RobotDynamics.LieState(::LieBody) = LieState(UnitQuaternion{Float64},(3,2,3))
+RobotDynamics.LieState(::LieBody) = LieState(QuatRotation{Float64}, (3, 2, 3))
 
 model = LieBody()
 @test s === LieState(model)
diff --git a/test/quadrotor.jl b/test/quadrotor.jl
index c74883b..1a4f702 100644
--- a/test/quadrotor.jl
+++ b/test/quadrotor.jl
@@ -10,7 +10,7 @@ in the positive z direction.
     Quadrotor(; kwargs...)
     Quadrotor{R}(; kwargs...)
 
-where `R <: Rotation{3}` and defaults to `UnitQuaternion{Float64}` if omitted. The keyword arguments are
+where `R <: Rotation{3}` and defaults to `QuatRotation{Float64}` if omitted. The keyword arguments are
 * `mass` - mass of the quadrotor, in kg (default = 0.5)
 * `J` - inertia of the quadrotor, in kg⋅m² (default = `Diagonal([0.0023, 0.0023, 0.004])`)
 * `gravity` - gravity vector, in kg/m² (default = [0,0,-9.81])
@@ -34,23 +34,23 @@ end
 RobotDynamics.control_dim(::Quadrotor) = 4
 
 function Quadrotor{R}(;
-        mass=0.5,
-        J=Diagonal(@SVector [0.0023, 0.0023, 0.004]),
-        gravity=SVector(0,0,-9.81),
-        motor_dist=0.1750,
-        kf=1.0,
-        km=0.0245,
-        bodyframe=false,
-        info=Dict{Symbol,Any}()) where R
-    Quadrotor{R}(13,4,mass,J,inv(J),gravity,motor_dist,kf,km,bodyframe,info)
+    mass=0.5,
+    J=Diagonal(@SVector [0.0023, 0.0023, 0.004]),
+    gravity=SVector(0, 0, -9.81),
+    motor_dist=0.1750,
+    kf=1.0,
+    km=0.0245,
+    bodyframe=false,
+    info=Dict{Symbol,Any}()) where {R}
+    Quadrotor{R}(13, 4, mass, J, inv(J), gravity, motor_dist, kf, km, bodyframe, info)
 end
 
-(::Type{Quadrotor})(;kwargs...) = Quadrotor{UnitQuaternion{Float64}}(;kwargs...)
+(::Type{Quadrotor})(; kwargs...) = Quadrotor{QuatRotation{Float64}}(; kwargs...)
 
 @inline RobotDynamics.velocity_frame(model::Quadrotor) = model.bodyframe ? :body : :world
 
 function trim_controls(model::Quadrotor)
-    @SVector fill(-model.gravity[3]*model.mass/4.0, control_dim(model))
+    @SVector fill(-model.gravity[3] * model.mass / 4.0, control_dim(model))
 end
 
 function RobotDynamics.forces(model::Quadrotor, x, u)
@@ -64,13 +64,13 @@ function RobotDynamics.forces(model::Quadrotor, x, u)
     w3 = u[3]
     w4 = u[4]
 
-    F1 = max(0,kf*w1);
-    F2 = max(0,kf*w2);
-    F3 = max(0,kf*w3);
-    F4 = max(0,kf*w4);
-    F = @SVector [0., 0., F1+F2+F3+F4] #total rotor force in body frame
+    F1 = max(0, kf * w1)
+    F2 = max(0, kf * w2)
+    F3 = max(0, kf * w3)
+    F4 = max(0, kf * w4)
+    F = @SVector [0.0, 0.0, F1 + F2 + F3 + F4] #total rotor force in body frame
 
-    m*g + q*F # forces in world frame
+    m * g + q * F # forces in world frame
 end
 
 function RobotDynamics.moments(model::Quadrotor, x, u)
@@ -83,16 +83,16 @@ function RobotDynamics.moments(model::Quadrotor, x, u)
     w3 = u[3]
     w4 = u[4]
 
-    F1 = max(0,kf*w1);
-    F2 = max(0,kf*w2);
-    F3 = max(0,kf*w3);
-    F4 = max(0,kf*w4);
+    F1 = max(0, kf * w1)
+    F2 = max(0, kf * w2)
+    F3 = max(0, kf * w3)
+    F4 = max(0, kf * w4)
 
-    M1 = km*w1;
-    M2 = km*w2;
-    M3 = km*w3;
-    M4 = km*w4;
-    tau = @SVector [L*(F2-F4), L*(F3-F1), (M1-M2+M3-M4)] #total rotor torque in body frame
+    M1 = km * w1
+    M2 = km * w2
+    M3 = km * w3
+    M4 = km * w4
+    tau = @SVector [L * (F2 - F4), L * (F3 - F1), (M1 - M2 + M3 - M4)] #total rotor torque in body frame
 end
 
 function RobotDynamics.wrenches(model::Quadrotor, x::SVector, u::SVector)
@@ -106,22 +106,22 @@ function RobotDynamics.wrenches(model::Quadrotor, x::SVector, u::SVector)
 
     # Calculate force and moments
     w = max.(u, 0)  # keep forces positive
-    fM = forceMatrix(model)*w
+    fM = forceMatrix(model) * w
     f = fM[1]
     M = @SVector [fM[2], fM[3], fM[4]]
-    e3 = @SVector [0,0,1]
-    F = mass*g - q*(f*e3)
-    return F,M
+    e3 = @SVector [0, 0, 1]
+    F = mass * g - q * (f * e3)
+    return F, M
 end
 
 function forceMatrix(model::Quadrotor)
     kf, km = model.kf, model.km
     L = model.motor_dist
     @SMatrix [
-        kf   kf   kf   kf;
-        0    L*kf 0   -L*kf;
-       -L*kf 0    L*kf 0;
-        km  -km   km  -km;
+        kf kf kf kf;
+        0 L*kf 0 -L*kf;
+        -L*kf 0 L*kf 0;
+        km -km km -km
     ]
 end
 
@@ -130,8 +130,8 @@ RobotDynamics.inertia(model::Quadrotor) = model.J
 RobotDynamics.inertia_inv(model::Quadrotor) = model.Jinv
 RobotDynamics.mass(model::Quadrotor) = model.mass
 
-function Base.zeros(model::Quadrotor{R}) where R
+function Base.zeros(model::Quadrotor{R}) where {R}
     x = RobotDynamics.build_state(model, zero(RBState))
-    u = @SVector fill(-model.mass*model.gravity[end]/4, 4)
-    return x,u
+    u = @SVector fill(-model.mass * model.gravity[end] / 4, 4)
+    return x, u
 end
diff --git a/test/rbstate.jl b/test/rbstate.jl
index 48179f7..56a6edb 100644
--- a/test/rbstate.jl
+++ b/test/rbstate.jl
@@ -3,9 +3,11 @@ using RobotDynamics
 using StaticArrays
 using Rotations
 using BenchmarkTools
+using Random
 
+Random.seed!(1)
 r = @SVector rand(3)
-q = rand(UnitQuaternion)
+q = rand(QuatRotation)
 v = @SVector rand(3)
 ω = @SVector rand(3)
 
@@ -20,7 +22,7 @@ x32 = RBState{Float32}(r, q, v, ω)
 @test x32 isa RBState{Float32}
 @test eltype(x32[1:3]) == Float32
 @test RD.position(x32) isa SVector{3,Float32}
-@test RD.orientation(x32) isa UnitQuaternion{Float32}
+@test RD.orientation(x32) isa QuatRotation{Float32}
 
 # Pass in other types of vectors
 x = RBState{Float64}(Vector(r), q, v, ω)
@@ -30,7 +32,7 @@ x = RBState{Float64}(Vector(r), q, Vector(v), Vector(ω))
 @test RD.linear_velocity(x) isa SVector{3,Float64}
 @test RD.angular_velocity(x) isa SVector{3,Float64}
 
-x = RBState{Float64}(view(r,:), q, view(Vector(v),1:3), Vector(ω))
+x = RBState{Float64}(view(r, :), q, view(Vector(v), 1:3), Vector(ω))
 @test RD.position(x) isa SVector{3,Float64}
 
 @test RD.RBState{Float32}(x) isa RBState{Float32}
@@ -40,21 +42,21 @@ x = RBState{Float64}(view(r,:), q, view(Vector(v),1:3), Vector(ω))
 
 # Pass in other rotations
 x2 = RD.RBState{Float64}(r, RotMatrix(q), v, ω)
-@test RD.orientation(x2) \ RD.orientation(x) ≈ one(UnitQuaternion)
-@test RD.orientation(x2) isa UnitQuaternion
+@test RD.orientation(x2) \ RD.orientation(x) ≈ one(QuatRotation)
+@test RD.orientation(x2) isa QuatRotation
 
 # Let the constructor figure out data type
 @test RBState(r, q, v, ω) isa RBState{Float64}
-@test RBState(Float32.(r), UnitQuaternion{Float32}(q), Float32.(v), Float32.(ω)) isa RBState{Float32}
+@test RBState(Float32.(r), QuatRotation{Float32}(q), Float32.(v), Float32.(ω)) isa RBState{Float32}
 @test RBState(Float32.(r), q, Float32.(v), Float32.(ω)) isa RBState{Float64}
-@test RBState(r, UnitQuaternion{Float32}(q), Float32.(v), Float32.(ω)) isa RBState{Float64}
+@test RBState(r, QuatRotation{Float32}(q), Float32.(v), Float32.(ω)) isa RBState{Float64}
 
 # Convert from another rotation type
 x = [r; Rotations.params(MRP(q)); v; ω]
-@test RBState(MRP, x) ≈ RBState(r,q,v,ω)
+@test RBState(MRP, x) ≈ RBState(r, q, v, ω)
 x = [r; Rotations.params(RodriguesParam(q)); v; ω]
-@test !(RBState(MRP, x) ≈ RBState(r,q,v,ω))
-@test RBState(RodriguesParam, x) ≈ RBState(r,q,v,ω)
+@test !(RBState(MRP, x) ≈ RBState(r, q, v, ω))
+@test RBState(RodriguesParam, x) ≈ RBState(r, q, v, ω)
 
 
 # Pass in a vector for the quaternion
@@ -62,15 +64,17 @@ q_ = Rotations.params(q)
 x = RBState(r, q_, v, ω)
 @test RD.orientation(x) ≈ q
 x = RBState(r, 2q_, v, ω)
-@test RD.orientation(x) ≈ 2q  # shouldnt renormalize
+@which RBState(r, 2q_, v, ω)
+@test Rotations.params(RD.orientation(x)) ≈ 2q_  # shouldnt renormalize
+Rotations.params(RD.orientation(x))
 @test RBState(Float32.(r), Float32.(q_), Float32.(v), Float32.(ω)) isa RBState{Float32}
 
 # Pass in a vector for the entire state
-using RobotDynamics: position, orientation, linear_velocity, angular_velocity 
+using RobotDynamics: position, orientation, linear_velocity, angular_velocity
 x_ = [r; 2q_; v; ω]
 x = RBState(x_)
 @test position(x) ≈ r
-@test orientation(x) ≈ 2q  # shouldnt renormalize
+@test Rotations.params(orientation(x)) ≈ 2q_  # shouldnt renormalize
 @test linear_velocity(x) ≈ v
 @test angular_velocity(x) ≈ ω
 
@@ -78,9 +82,10 @@ x = RBState(x_)
 @test RBState(Vector(x_)) isa RBState{Float64}
 
 # Test comparison (with double-cover)
-q = rand(UnitQuaternion)
+q = rand(QuatRotation)
 x1 = RBState(r, q, v, ω)
-x2 = RBState(r, -q, v, ω)
+x2 = RBState(r, -q.q, v, ω)
+@which RBState(r, -q.q, v, ω)
 @test x1[4:7] ≈ -x2[4:7]
 @test x1 ≈ x2
 @test !(SVector(x1) ≈ SVector(x2))
@@ -93,22 +98,22 @@ x = RBState(r, q, v, ω)
 @test x[11:13] ≈ ω
 @test x[4] ≈ q.w
 
-@test getindex(x,2) == r[2]
-@test getindex(x,8) == v[1]
-@test getindex(x,13) == ω[3]
+@test getindex(x, 2) == r[2]
+@test getindex(x, 8) == v[1]
+@test getindex(x, 13) == ω[3]
 
 # Renorm
-x2 = RBState(r,2*q,v,ω)
-@test norm(orientation(x2)) ≈ 2
+x2 = RBState(r, 2 * q.q, v, ω)
+@test norm(orientation(x2).q) ≈ 2
 x = RobotDynamics.renorm(x2)
-@test norm(orientation(x2)) ≈ 2
-@test norm(orientation(x)) ≈ 1
+@test norm(orientation(x2).q) ≈ 2
+@test norm(orientation(x).q) ≈ 1
 @test isrotation(orientation(x))
 
 # Test zero constructor
 x0 = zero(RBState)
 @test position(x0) ≈ zero(r)
-@test orientation(x0) ≈ one(UnitQuaternion)
+@test orientation(x0) ≈ one(QuatRotation)
 @test linear_velocity(x0) ≈ zero(v)
 @test angular_velocity(x0) ≈ zero(v)
 @test zero(RBState) isa RBState{Float64}
@@ -154,8 +159,8 @@ q2 = orientation(x2)
 @test Rotations.params(RodriguesParam(q2) \ RodriguesParam(q1)) ≈ dx[4:6]
 
 # Test randbetween
-xmin = RBState(fill(-1,3), one(UnitQuaternion), fill(-2,3), fill(-3,3))
-xmax = RBState(fill(+1,3), one(UnitQuaternion), fill(+2,3), fill(+3,3))
+xmin = RBState(fill(-1, 3), one(QuatRotation), fill(-2, 3), fill(-3, 3))
+xmax = RBState(fill(+1, 3), one(QuatRotation), fill(+2, 3), fill(+3, 3))
 @test randbetween(xmin, xmax) isa RBState{Float64}
 for k = 1:10
     local x = randbetween(xmin, xmax)
diff --git a/test/rigid_body_jacobians.jl b/test/rigid_body_jacobians.jl
index 2dc36b3..f9afbbf 100644
--- a/test/rigid_body_jacobians.jl
+++ b/test/rigid_body_jacobians.jl
@@ -1,7 +1,7 @@
 using RobotDynamics: orientation
 
 # Set up model
-model = Body{UnitQuaternion{Float64}}()
+model = Body{QuatRotation{Float64}}()
 @test RD.dims(model) == (13, 6, 13)
 x, u = rand(model)
 t, dt = 0, 0.1
@@ -32,7 +32,7 @@ RD.jacobian!(RD.StaticReturn(), RD.ForwardAD(), model, F2, y2, z)
 
 ω = RD.angular_velocity(model, x)
 ForwardDiff.jacobian(
-    q -> Rotations.kinematics(UnitQuaternion(q, false), ω),
+    q -> Rotations.kinematics(QuatRotation(q, false), ω),
     Rotations.params(q),
 )
 Rotations.kinematics(q, ω)
@@ -73,14 +73,14 @@ G0 = Rotations.∇differential(q)
 G = zeros(RD.state_dim(model), RobotDynamics.errstate_dim(model))
 @test size(G) == (13, 12)
 RD.errstate_jacobian!(model, G, z)
-@test G ≈ cat(I(3), G0, I(6), dims = (1, 2))
+@test G ≈ cat(I(3), G0, I(6), dims=(1, 2))
 
 b = @SVector rand(13)
 ∇G0 = Rotations.∇²differential(q, b[SA[4, 5, 6, 7]])
 ∇G = zeros(RD.errstate_dim(model), RD.errstate_dim(model))
 @test size(∇G) == (12, 12)
 RobotDynamics.∇errstate_jacobian!(model, ∇G, x, b)
-@test ∇G ≈ cat(zeros(3, 3), ∇G0, zeros(6, 6), dims = (1, 2))
+@test ∇G ≈ cat(zeros(3, 3), ∇G0, zeros(6, 6), dims=(1, 2))
 
 # @btime RobotDynamics.state_diff_jacobian!($G, $model, $z)
 # @btime RobotDynamics.∇²differential!($∇G, $model, $x, $b)
\ No newline at end of file
diff --git a/test/rigidbody_test.jl b/test/rigidbody_test.jl
index 6c93e41..034a79b 100644
--- a/test/rigidbody_test.jl
+++ b/test/rigidbody_test.jl
@@ -9,16 +9,16 @@ using ForwardDiff
 using RobotDynamics: mass, inertia
 
 # Temporary fix for Rotations
-# function Rotations.∇rotate(q::UnitQuaternion, r::AbstractVector)
+# function Rotations.∇rotate(q::QuatRotation, r::AbstractVector)
 #     Rotations.check_length(r, 3)
-#     rhat = UnitQuaternion(zero(eltype(r)), r[1], r[2], r[3], false)
+#     rhat = QuatRotation(zero(eltype(r)), r[1], r[2], r[3], false)
 #     R = Rotations.rmult(q)
 #     2*Rotations.vmat()*Rotations.rmult(q)'*Rotations.rmult(rhat)
 # end
 
 v = @SVector rand(3)
-q = rand(UnitQuaternion)
-ForwardDiff.jacobian(q->UnitQuaternion(q,false)*v, Rotations.params(q)) ≈ Rotations.∇rotate(q,v)
+q = rand(QuatRotation)
+ForwardDiff.jacobian(q -> QuatRotation(q, false) * v, Rotations.params(q)) ≈ Rotations.∇rotate(q, v)
 
 RD.@autodiff struct Body{R} <: RobotDynamics.RigidBody{R} end
 RobotDynamics.control_dim(::Body) = 6
@@ -32,9 +32,9 @@ end
 
 function RobotDynamics.wrench_jacobian!(F, model::Body, z::RD.AbstractKnotPoint)
     inds = RobotDynamics.gen_inds(model)
-    r,q,v,ω = RobotDynamics.parse_state(model, state(z))
+    r, q, v, ω = RobotDynamics.parse_state(model, state(z))
     u = control(z)
-    i3 = SA[1,2,3]
+    i3 = SA[1, 2, 3]
     f = u[i3]
     F[i3, inds.q] .= Rotations.∇rotate(q, f)
     F[i3, inds.u[i3]] .= RotMatrix(q)
@@ -45,43 +45,43 @@ function RobotDynamics.wrench_jacobian!(F, model::Body, z::RD.AbstractKnotPoint)
 end
 
 function testind(model)
-    ir,iq,iv,iω,iu = gen_inds(model)
+    ir, iq, iv, iω, iu = gen_inds(model)
     @SVector ones(control_dim(model))
 end
 
 
 RobotDynamics.mass(::Body) = 2.0
-RobotDynamics.inertia(::Body) = Diagonal(SA[2,3,1.])
-RobotDynamics.wrench_sparsity(::Body) = SA[false true  false false true;
-                                           false false false false true]
+RobotDynamics.inertia(::Body) = Diagonal(SA[2, 3, 1.0])
+RobotDynamics.wrench_sparsity(::Body) = SA[false true false false true;
+    false false false false true]
 
 #---
-model = Body{UnitQuaternion{Float64}}()
-@test RD.dims(model) == (13,6,13)
-x,u = rand(model)
+model = Body{QuatRotation{Float64}}()
+@test RD.dims(model) == (13, 6, 13)
+x, u = rand(model)
 q = orientation(model, x)
 @test Rotations.params(q) == x[4:7]
-@test norm(q) ≈ 1
-z = RD.KnotPoint(x,u,0.0,0.01)
+@test norm(q.q) ≈ 1
+z = RD.KnotPoint(x, u, 0.0, 0.01)
 @test length(x) == 13
 @test length(u) == 6
 @test norm(x[4:7]) ≈ 1
-@test RobotDynamics.LieState(model) === RobotDynamics.LieState(UnitQuaternion{Float64},(3,6))
-@test RobotDynamics.rotation_type(model) == UnitQuaternion{Float64}
+@test RobotDynamics.LieState(model) === RobotDynamics.LieState(QuatRotation{Float64}, (3, 6))
+@test RobotDynamics.rotation_type(model) == QuatRotation{Float64}
 
 # Test initializers
-x0,u0 = zeros(model)
+x0, u0 = zeros(model)
 @test RBState(model, x0) ≈ zero(RBState)
 @test u0 ≈ zeros(RD.control_dim(model))
-@test x0[4:7] ≈ [1,0,0,0]
+@test x0[4:7] ≈ [1, 0, 0, 0]
 
 # Test diferent rotations
-for R in [UnitQuaternion{Float64}, MRP{Float64}, RodriguesParam{Float64}]
+for R in [QuatRotation{Float64}, MRP{Float64}, RodriguesParam{Float64}]
     local model = Body{R}()
-	@test RD.state_dim(model) == 9 + Rotations.params(R)
-	RobotDynamics.rotation_type(model) == R
-	local x0,u0 = zeros(model)
-	@test RBState(model, x0) ≈ zero(RBState)
+    @test RD.state_dim(model) == 9 + Rotations.params(R)
+    RobotDynamics.rotation_type(model) == R
+    local x0, u0 = zeros(model)
+    @test RBState(model, x0) ≈ zero(RBState)
 end
 
 # Test gen_inds
@@ -98,8 +98,8 @@ inds = RobotDynamics.gen_inds(model2)
 @test inds.u == 13:18
 
 # Test state building methods
-r,q,v,ω = RobotDynamics.parse_state(model, x)
-ir,iq,iv,iω,iu = RobotDynamics.gen_inds(model)
+r, q, v, ω = RobotDynamics.parse_state(model, x)
+ir, iq, iv, iω, iu = RobotDynamics.gen_inds(model)
 @test r == x[ir]
 @test Rotations.params(q) == x[iq]
 @test v == x[iv]
@@ -122,13 +122,13 @@ x2 = [r; g; v; ω]
 
 @test RobotDynamics.fill_state(model, 1, 2, 3, 4) isa SVector{13,Int}
 @test RobotDynamics.fill_state(model2, 1, 2, 3, 4) isa SVector{12,Int}
-@test RobotDynamics.fill_state(model, 1, 2, 3, 4.) isa SVector{13,Float64}
+@test RobotDynamics.fill_state(model, 1, 2, 3, 4.0) isa SVector{13,Float64}
 x_ = RobotDynamics.fill_state(model, 1, 2, 3, 4)
-@test position(model, x_) ≈ fill(1,3)
-@test Rotations.params(orientation(model, x_)) ≈ fill(2,4)
-@test linear_velocity(model, x_) ≈ fill(3,3)
-@test angular_velocity(model, x_) ≈ fill(4,3)
-@test RobotDynamics.fill_state(model2, 1, 2, 3, 4.)[4:6] ≈ fill(2,3)
+@test position(model, x_) ≈ fill(1, 3)
+@test Rotations.params(orientation(model, x_)) ≈ fill(2, 4)
+@test linear_velocity(model, x_) ≈ fill(3, 3)
+@test angular_velocity(model, x_) ≈ fill(4, 3)
+@test RobotDynamics.fill_state(model2, 1, 2, 3, 4.0)[4:6] ≈ fill(2, 3)
 
 
 # Test dynamics
@@ -141,11 +141,11 @@ x_ = RBState(x)
 @test position(xdot) ≈ linear_velocity(x_)
 @test Rotations.params(orientation(xdot)) ≈ Rotations.kinematics(orientation(x_), angular_velocity(x_))
 ξ = RobotDynamics.wrenches(model, state(z), control(z), 0.0)
-F = ξ[SA[1,2,3]]
-T = ξ[SA[4,5,6]]
-@test linear_velocity(xdot) ≈ F/mass(model)
+F = ξ[SA[1, 2, 3]]
+T = ξ[SA[4, 5, 6]]
+@test linear_velocity(xdot) ≈ F / mass(model)
 @test angular_velocity(xdot) ≈
-	inertia(model) \ (T - angular_velocity(x_) × (inertia(model) * angular_velocity(x_)))
+      inertia(model) \ (T - angular_velocity(x_) × (inertia(model) * angular_velocity(x_)))
 
 # Test body-frame velocity
 RobotDynamics.velocity_frame(::Body) = :body
@@ -159,7 +159,7 @@ q = orientation(x_)
 @test Rotations.params(orientation(xdot)) ≈ Rotations.kinematics(orientation(x_), angular_velocity(x_))
 @test linear_velocity(xdot) ≈ q \ (F / mass(model)) - angular_velocity(x_) × linear_velocity(x_)
 @test angular_velocity(xdot) ≈
-	inertia(model) \ (T - angular_velocity(x_) × (inertia(model) * angular_velocity(x_)))
+      inertia(model) \ (T - angular_velocity(x_) × (inertia(model) * angular_velocity(x_)))