Rotations update

Project.toml

@@ -1,7 +1,7 @@
 name = "Altro"
 uuid = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73"
 authors = ["Brian Jackson <[email protected]>"]
-version = "0.5.0"
+version = "0.5.1"
 
 [deps]
 BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"
@@ -30,13 +30,13 @@ Crayons = "4"
 FiniteDiff = "2"
 Formatting = "0.4"
 ForwardDiff = "0.10"
-Interpolations = "0.12,0.13"
+Interpolations = "0.12,0.13,0.14"
 Octavian = "0.3"
-RobotDynamics = "0.4"
+RobotDynamics = "0.4.8"
 RobotZoo = "0.3"
-Rotations = "~1"
+Rotations = "1"
 SolverLogging = "0.2"
 StaticArrays = "1"
 TimerOutputs = "0.5"
-TrajectoryOptimization = "0.7"
+TrajectoryOptimization = "0.7.1"
 julia = "1"

problems/airplane.jl

@@ -2,100 +2,100 @@ const RD = RobotDynamics
 using TrajectoryOptimization: QuatLQRCost, ErrorQuadratic, QuatVecEq
 
 function YakProblems(;
-        integration=RD.RK4,
-        N = 101,
-        vecstate=false,
-        scenario=:barrellroll, 
-        costfun=:Quadratic, 
-        termcon=:goal,
-        quatnorm=:none,
-        kwargs...
-    )
-    model = RobotZoo.YakPlane(UnitQuaternion)
+    integration=RD.RK4,
+    N=101,
+    vecstate=false,
+    scenario=:barrellroll,
+    costfun=:Quadratic,
+    termcon=:goal,
+    quatnorm=:none,
+    kwargs...
+)
+    model = RobotZoo.YakPlane(QuatRotation)
 
     opts = SolverOptions(
-        cost_tolerance_intermediate = 1e-1,
-        penalty_scaling = 10.,
-        penalty_initial = 10.;
+        cost_tolerance_intermediate=1e-1,
+        penalty_scaling=10.0,
+        penalty_initial=10.0;
         kwargs...
     )
 
     s = RD.LieState(model)
-    n,m = RD.dims(model)
+    n, m = RD.dims(model)
     rsize = RD.state_dim(model) - 9
-    vinds = SA[1,2,3,8,9,10,11,12,13]
+    vinds = SA[1, 2, 3, 8, 9, 10, 11, 12, 13]
 
     # Discretization
     tf = 1.25
-    dt = tf/(N-1)
+    dt = tf / (N - 1)
 
     if scenario == :barrellroll
-        ey = @SVector [0,1,0.]
+        ey = @SVector [0, 1, 0.0]
 
         # Initial and final condition
-        p0 = MRP(0.997156, 0., 0.075366) # initial orientation
-        pf = MRP(0., -0.0366076, 0.) # final orientation (upside down)
+        p0 = MRP(0.997156, 0.0, 0.075366) # initial orientation
+        pf = MRP(0.0, -0.0366076, 0.0) # final orientation (upside down)
 
-        x0 = RD.build_state(model, [-3,0,1.5], p0, [5,0,0], [0,0,0])
-        utrim  = @SVector  [41.6666, 106, 74.6519, 106]
-        xf = RD.build_state(model, [3,0,1.5], pf, [5,0,0], [0,0,0])
+        x0 = RD.build_state(model, [-3, 0, 1.5], p0, [5, 0, 0], [0, 0, 0])
+        utrim = @SVector [41.6666, 106, 74.6519, 106]
+        xf = RD.build_state(model, [3, 0, 1.5], pf, [5, 0, 0], [0, 0, 0])
 
         # Xref trajectory
         x̄0 = RBState(model, x0)
         x̄f = RBState(model, xf)
         Xref = map(1:N) do k
-            x̄0 + (x̄f - x̄0)*((k-1)/(N-1))
+            x̄0 + (x̄f - x̄0) * ((k - 1) / (N - 1))
         end
 
         # Objective
-        Qf_diag = RD.fill_state(model, 100, 500, 100, 100.)
+        Qf_diag = RD.fill_state(model, 100, 500, 100, 100.0)
         Q_diag = RD.fill_state(model, 0.1, 0.1, 0.1, 0.1)
         Qf = Diagonal(Qf_diag)
         Q = Diagonal(Q_diag)
-        R = Diagonal(@SVector fill(1e-3,4))
+        R = Diagonal(@SVector fill(1e-3, 4))
         if quatnorm == :slack
             m += 1
             R = Diagonal(push(R.diag, 1e-6))
             utrim = push(utrim, 0)
         end
         if costfun == :Quadratic
             costfuns = map(Xref) do xref
-                LQRCost(Q*dt, R*dt, xf, utrim)
+                LQRCost(Q * dt, R * dt, xf, utrim)
             end
-            costfun = LQRCost(Q*dt, R*dt, xf, utrim)
+            costfun = LQRCost(Q * dt, R * dt, xf, utrim)
             costterm = LQRCost(Qf, R, xf, utrim)
             costfuns[end] = costterm
         elseif costfun == :QuatLQR
             costfuns = map(Xref) do xref
-                QuatLQRCost(Q*dt, R*dt, xf, utrim, w=0.1)
+                QuatLQRCost(Q * dt, R * dt, xf, utrim, w=0.1)
             end
-            costfun = QuatLQRCost(Q*dt, R*dt, xf, utrim; w=0.1)
+            costfun = QuatLQRCost(Q * dt, R * dt, xf, utrim; w=0.1)
             costterm = QuatLQRCost(Qf, R, xf, utrim; w=200.0)
             costfuns[end] = costterm
         elseif costfun == :LieLQR
-            costfun = LieLQR(s, Q*dt, R*dt, xf, utrim)
-            costterm = LieLQR(s, Qf, R*dt, xf, utrim)
+            costfun = LieLQR(s, Q * dt, R * dt, xf, utrim)
+            costterm = LieLQR(s, Qf, R * dt, xf, utrim)
         elseif costfun == :ErrorQuadratic
             costfuns = map(Xref) do xref
-                ErrorQuadratic(model, Q*dt, R*dt, xref, utrim)
+                ErrorQuadratic(model, Q * dt, R * dt, xref, utrim)
             end
-            costfun = ErrorQuadratic(model, Q*dt, R*dt, xf, utrim)
-            costterm = ErrorQuadratic(model, Qf*10, R, xf, utrim)
+            costfun = ErrorQuadratic(model, Q * dt, R * dt, xf, utrim)
+            costterm = ErrorQuadratic(model, Qf * 10, R, xf, utrim)
             costfuns[end] = costterm
         end
         obj = Objective(costfuns)
 
         # Constraints
-        conSet = ConstraintList(n,m,N)
-        vecgoal = GoalConstraint(xf, vinds) 
+        conSet = ConstraintList(n, m, N)
+        vecgoal = GoalConstraint(xf, vinds)
         rot_diffmethod = RD.UserDefined()
         if termcon == :goal
-            rotgoal = GoalConstraint(xf, SA[4,5,6,7])
+            rotgoal = GoalConstraint(xf, SA[4, 5, 6, 7])
         elseif termcon == :quatvec
             rotgoal = QuatVecEq(n, m, pf)
             rot_diffmethod = RD.ForwardAD()
         elseif termcon == :quaterr
-            rotgoal = QuatErr(n, UnitQuaternion(pf), SA[4,5,6,7])
+            rotgoal = QuatErr(n, QuatRotation(pf), SA[4, 5, 6, 7])
         else
             throw(ArgumentError("$termcon is not a known option for termcon. Options are :goal, :quatvec, :quaterr"))
         end
@@ -110,7 +110,7 @@ function YakProblems(;
     U0 = [copy(utrim) for k = 1:N-1]
 
     # Use a standard model (no special handling of rotation states)
-    if quatnorm == :renorm 
+    if quatnorm == :renorm
         model = QuatRenorm(model)
     elseif quatnorm == :slack
         model = QuatSlackModel(model)

problems/quadrotor.jl

@@ -1,49 +1,49 @@
-function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64};
-        costfun=:Quadratic, normcon=false)
+function Quadrotor(scenario=:zigzag, Rot=QuatRotation{Float64};
+    costfun=:Quadratic, normcon=false)
     if scenario == :zigzag
         model = RobotZoo.Quadrotor{Rot}()
-        n,m = RD.dims(model)
+        n, m = RD.dims(model)
 
         opts = SolverOptions(
-            penalty_scaling=100.,
+            penalty_scaling=100.0,
             penalty_initial=0.1,
         )
 
         # discretization
         N = 101 # number of knot points
         tf = 5.0
-        dt = tf/(N-1) # total time
+        dt = tf / (N - 1) # total time
 
         # Initial condition
-        x0_pos = @SVector [0., -10., 1.]
-        x0 = RobotDynamics.build_state(model, x0_pos, UnitQuaternion(I), zeros(3), zeros(3))
+        x0_pos = @SVector [0.0, -10.0, 1.0]
+        x0 = RobotDynamics.build_state(model, x0_pos, QuatRotation(I), zeros(3), zeros(3))
 
         # cost
         costfun == :QuatLQR ? sq = 0 : sq = 1
-        rm_quat = @SVector [1,2,3,4,5,6,8,9,10,11,12,13]
-        Q_diag = Dynamics.fill_state(model, 1e-5, 1e-5*sq, 1e-3, 1e-3)
+        rm_quat = @SVector [1, 2, 3, 4, 5, 6, 8, 9, 10, 11, 12, 13]
+        Q_diag = Dynamics.fill_state(model, 1e-5, 1e-5 * sq, 1e-3, 1e-3)
         Q = Diagonal(Q_diag)
-        R = Diagonal(@SVector fill(1e-4,m))
-        q_nom = UnitQuaternion(I)
+        R = Diagonal(@SVector fill(1e-4, m))
+        q_nom = QuatRotation(I)
         v_nom, ω_nom = zeros(3), zeros(3)
         x_nom = Dynamics.build_state(model, zeros(3), q_nom, v_nom, ω_nom)
 
         if costfun == :QuatLQR
-            cost_nom = QuatLQRCost(Q*dt, R*dt, x_nom, w=0.0)
+            cost_nom = QuatLQRCost(Q * dt, R * dt, x_nom, w=0.0)
         elseif costfun == :ErrorQuad
-            cost_nom = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat])*dt, R*dt, x_nom)
+            cost_nom = ErrorQuadratic(model, Diagonal(Q_diag[rm_quat]) * dt, R * dt, x_nom)
         else
-            cost_nom = LQRCost(Q*dt, R*dt, x_nom)
+            cost_nom = LQRCost(Q * dt, R * dt, x_nom)
         end
 
         # waypoints
-        wpts = [(@SVector [10,0,1.]),
-                (@SVector [-10,0,1.]),
-                (@SVector [0,10,1.])]
+        wpts = [(@SVector [10, 0, 1.0]),
+            (@SVector [-10, 0, 1.0]),
+            (@SVector [0, 10, 1.0])]
         times = [33, 66, 101]
-        Qw_diag = Dynamics.fill_state(model, 1e3,1*sq,1,1)
-        Qf_diag = Dynamics.fill_state(model, 10., 100*sq, 10, 10)
-        xf = Dynamics.build_state(model, wpts[end], UnitQuaternion(I), zeros(3), zeros(3))
+        Qw_diag = Dynamics.fill_state(model, 1e3, 1 * sq, 1, 1)
+        Qf_diag = Dynamics.fill_state(model, 10.0, 100 * sq, 10, 10)
+        xf = Dynamics.build_state(model, wpts[end], QuatRotation(I), zeros(3), zeros(3))
 
         costs = map(1:length(wpts)) do i
             r = wpts[i]
@@ -52,11 +52,11 @@ function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64};
                 Q = Diagonal(Qf_diag)
                 w = 40.0
             else
-                Q = Diagonal(1e-3*Qw_diag) * dt
+                Q = Diagonal(1e-3 * Qw_diag) * dt
                 w = 0.1
             end
             if costfun == :QuatLQR
-                QuatLQRCost(Q, R*dt, xg, w=w)
+                QuatLQRCost(Q, R * dt, xg, w=w)
             elseif costfun == :ErrorQuad
                 Qd = diag(Q)
                 ErrorQuadratic(model, Diagonal(Qd[rm_quat]), R, xg)
@@ -66,7 +66,7 @@ function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64};
         end
 
         costs_all = map(1:N) do k
-            i = findfirst(x->(x ≥ k), times)
+            i = findfirst(x -> (x ≥ k), times)
             if k ∈ times
                 costs[i]
             else
@@ -77,20 +77,20 @@ function Quadrotor(scenario=:zigzag, Rot=UnitQuaternion{Float64};
         obj = Objective(costs_all)
 
         # Initialization
-        u0 = @SVector fill(0.5*9.81/4, m)
+        u0 = @SVector fill(0.5 * 9.81 / 4, m)
         U_hover = [copy(u0) for k = 1:N-1] # initial hovering control trajectory
 
         # Constraints
-        conSet = ConstraintList(n,m,N)
+        conSet = ConstraintList(n, m, N)
         if normcon
             if use_rot == :slack
                 add_constraint!(conSet, QuatSlackConstraint(), 1:N-1)
             else
                 add_constraint!(conSet, QuatNormConstraint(), 1:N-1)
-                u0 = [u0; (@SVector [1.])]
+                u0 = [u0; (@SVector [1.0])]
             end
         end
-        bnd = BoundConstraint(n,m, u_min=0.0, u_max=12.0)
+        bnd = BoundConstraint(n, m, u_min=0.0, u_max=12.0)
         add_constraint!(conSet, bnd, 1:N-1)
 
         # Problem