Merge pull request #7 from janbruedigam/small_fixes

Small fixes

Author: janbruedigam

src/ConstrainedControl.jl

@@ -5,6 +5,7 @@ using ConstrainedDynamics: svcat, szeros
 using LinearAlgebra
 using StaticArrays
 using Rotations
+using Rotations: rotation_error
 
 export PID,
     LQR,

src/control/lqr.jl

@@ -15,6 +15,33 @@ mutable struct LQR{T,N,NK} <: Controller
     control!::Function
 
 
+    function LQR(A, Bu, Bλ, G, Q, R, horizon, eqcids, xd, vd, qd, ωd, Fτd, Δt, ::Type{T}) where {T}
+        Q = cat(Q...,dims=(1,2))
+        R = cat(R...,dims=(1,2))
+
+        N = horizon/Δt
+        if N<Inf
+            N = Integer(ceil(horizon/Δt))
+            Ntemp = N
+        else
+            Ntemp = Integer(ceil(10/Δt)) # 10 second time horizon as maximal horizon for convergence for Inf
+        end
+
+        # calculate K
+        if size(G)[1] == 0
+            @assert size(Bλ)[2] ==0
+            Ku = dlqr(A, Bu, Q, R, N) # can be calculated directly
+        else
+            Ku = dlqr(A, Bu, Bλ, G, Q, R, Ntemp)
+            if N == Inf
+                Ku[1] != Ku[2] && @info "Riccati recursion did not converge."
+                Ku = [Ku[1]]
+            end
+        end
+        
+        new{T, N, size(Ku[1][1])[2]}(Ku, xd, vd, qd, ωd, eqcids, Fτd, control_lqr!)
+    end
+
     function LQR(mechanism::Mechanism{T,Nn,Nb}, bodyids::AbstractVector{<:Integer}, eqcids::AbstractVector{<:Integer},
             Q::Vector{<:AbstractMatrix{T}}, R::Vector{<:AbstractMatrix{T}}, horizon;
             xd::Vector{<:AbstractVector{T}} = [SA{T}[0; 0; 0] for i=1:Nb], 
@@ -27,28 +54,10 @@ mutable struct LQR{T,N,NK} <: Controller
         @assert length(bodyids) == length(Q) == length(xd) == length(vd) == length(qd) == length(ωd) == Nb "Missmatched length for bodies"
         @assert length(eqcids) == length(R) == length(Fτd) "Missmatched length for constraints"
 
-        Δt = mechanism.Δt
-        
-        N = horizon/Δt
-        if N<Inf
-            N = Integer(ceil(horizon/Δt))
-        end
-
         # linearize        
         A, Bu, Bλ, G = linearsystem(mechanism, xd, vd, qd, ωd, Fτd, bodyids, eqcids)
 
-        Q = cat(Q...,dims=(1,2))
-        R = cat(R...,dims=(1,2))
-
-        # calculate K
-        if size(G)[1] == 0
-            @assert size(Bλ)[2] ==0
-            Ku = dlqr(A, Bu, Q, R, N)
-        else
-            Ku = dlqr(A, Bu, Bλ, G, Q, R, N)
-        end
-        
-        new{T, N, size(Ku[1][1])[2]}(Ku, xd, vd, qd, ωd, eqcids, Fτd, control_lqr!)
+        LQR(A, Bu, Bλ, G, Q, R, horizon, eqcids, xd, vd, qd, ωd, Fτd, mechanism.Δt, T)
     end
 
     function LQR(mechanism::Mechanism{T,Nn,Nb}, controlledids::AbstractVector{<:Integer}, controlids::AbstractVector{<:Integer},
@@ -61,35 +70,19 @@ mutable struct LQR{T,N,NK} <: Controller
     @assert length(controlledids) == length(Q) == length(xθd) == length(vωd) == Nb "Missmatched length for bodies"
     @assert length(controlids) == length(R) == length(Fτd) "Missmatched length for constraints"
 
-    Δt = mechanism.Δt
-    
-    N = horizon/Δt
-    if N<Inf
-        N = Integer(ceil(horizon/Δt))
-    end
-
     # linearize        
     A, Bu, Bλ, G, xd, vd, qd, ωd = linearsystem(mechanism, xθd, vωd, Fτd, controlledids, controlids)
 
     Q = [diagm(ones(12))*Q[i] for i=1:length(Q)]
     R = [diagm(ones(1))*R[i] for i=1:length(R)]
-    Q = cat(Q...,dims=(1,2))
-    R = cat(R...,dims=(1,2))
 
-    # calculate K
-    if size(G)[1] == 0
-        @assert size(Bλ)[2] ==0
-        Ku = dlqr(A, Bu, Q, R, N)
-    else
-        Ku = dlqr(A, Bu, Bλ, G, Q, R, N)
-    end
-    
-    new{T, N, size(Ku[1][1])[2]}(Ku, xd, vd, qd, ωd, controlids, [[Fτd[i]] for i=1:length(Fτd)], control_lqr!)
+    LQR(A, Bu, Bλ, G, Q, R, horizon, controlids, xd, vd, qd, ωd, [[Fτd[i]] for i=1:length(Fτd)], mechanism.Δt, T)
 end
 end
 
 function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,N}, k) where {T,Nn,Nb,N}
     Δz = zeros(T,Nb*12)
+    qvm = QuatVecMap()
     for (id,body) in pairs(mechanism.bodies)
         colx = (id-1)*12+1:(id-1)*12+3
         colv = (id-1)*12+4:(id-1)*12+6
@@ -99,7 +92,8 @@ function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,N}, k) where {T,
         state = body.state
         Δz[colx] = state.xsol[2]-lqr.xd[id]
         Δz[colv] = state.vsol[2]-lqr.vd[id]
-        Δz[colq] = ConstrainedDynamics.VLᵀmat(lqr.qd[id]) * Rotations.params(state.qsol[2])
+        # Δz[colq] = ConstrainedDynamics.VLᵀmat(lqr.qd[id]) * Rotations.params(state.qsol[2])
+        Δz[colq] = rotation_error(state.qsol[2],lqr.qd[id],qvm)
         Δz[colω] = state.ωsol[2]-lqr.ωd[id]
     end
 
@@ -115,6 +109,7 @@ end
 
 function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,Inf}, k) where {T,Nn,Nb}
     Δz = zeros(T,Nb*12)
+    qvm = QuatVecMap()
     for (id,body) in pairs(mechanism.bodies)
         colx = (id-1)*12+1:(id-1)*12+3
         colv = (id-1)*12+4:(id-1)*12+6
@@ -124,7 +119,7 @@ function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,Inf}, k) where {
         state = body.state
         Δz[colx] = state.xsol[2]-lqr.xd[id]
         Δz[colv] = state.vsol[2]-lqr.vd[id]
-        Δz[colq] = ConstrainedDynamics.VLᵀmat(lqr.qd[id]) * Rotations.params(state.qsol[2])
+        Δz[colq] = rotation_error(state.qsol[2],lqr.qd[id],qvm)
         Δz[colω] = state.ωsol[2]-lqr.ωd[id]
     end
 
@@ -147,12 +142,6 @@ function dlqr(A,B,Q,R,N)
 end
 
 function dlqr(A,Bu,Bλ,G,Q,R,N)
-    infflag = false
-    if N == Inf
-        infflag = true
-        N = 1000
-    end
-
     mx = size(A)[2]
     mu = size(Bu)[2]
     mλ = size(Bλ)[2]
@@ -182,19 +171,15 @@ function dlqr(A,Bu,Bλ,G,Q,R,N)
         Abar = A-Bu*Kuk-Bλ*Kλk
         Pkp1 = Q + Kuk'*R*Kuk + Abar'*Pk*Abar
 
-        if infflag && norm(Pk-Pkp1) < 1e-5
+        if norm(Pk-Pkp1) < 1e-5
             break
         end
 
         Pk = Pkp1
     end
 
-    if infflag
-        if k==1
-            @info "Riccati recursion did not converge."
-        else
-            Ku = [Ku[k]]
-        end
+    for k2=k-1:-1:1
+        Ku[k2] = Ku[k2+1]
     end
 
     return Ku