Merge pull request #30 from RoboticExplorationLab/upgrade_rotations

Change UnitQuaternion to QuatRotation

Author: bjack205

Project.toml

@@ -1,13 +1,14 @@
 name = "RobotDynamics"
 uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1"
 authors = ["Brian Jackson <[email protected]>"]
-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"

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)

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,27 +35,27 @@ 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)
 @test length(s) == 14
 
 ω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))

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

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)