First commit

Project.toml

@@ -3,6 +3,12 @@ uuid = "92ba2ec4-d08f-4ce1-9cdf-fe8c7bb13e53"
 authors = ["F. Capolupo"]
 version = "0.1.0"
 
+[deps]
+ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e"
+GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a"
+JTools = "ab7590e3-7970-434e-bf9d-2a5e455e45fa"
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+
 [compat]
 julia = "1.10"
 

src/SpacecraftBuilder.jl

@@ -1,5 +1,20 @@
 module SpacecraftBuilder
 
-# Write your package code here.
+using JTools
+using LinearAlgebra
+using GLMakie
+using ControlSystems
+
+export verifyInertia
+include("utils.jl")
+
+export NoGeometry, PlanarPlate, Cuboid, Cylinder, Sphere
+include("scGeometry.jl")
+
+export RigidElement, FlexibleElement
+include("scElements.jl")
+
+export build, buildss
+include("scBuild.jl")
 
 end

src/scBuild.jl

@@ -0,0 +1,152 @@
+function initElement(;
+        ID::String="Element",
+        mass::Float64=0.0,
+        inertiaE_E::Matrix{Float64}=zeros(3, 3),        # [OPT1] Inertia
+        inertiaG_E::Matrix{Float64}=zeros(3, 3),        # [OPT2] Inertia
+        geometry::SpacecraftGeometry=NoGeometry(),
+        R_OE::Matrix{Float64}=Matrix(1.0I, 3, 3),
+        posEG_E::Vector{Float64}=zeros(3),
+        posOE_O::Vector{Float64}=zeros(3),
+        ω::Vector{Float64}=[NaN],
+        ξ::Vector{Float64}=[NaN],
+        LE_E::Matrix{Float64}=zeros(length(ω), 6),      # [OPT1] Modal participation matrix = [Translation, Rotation]
+        LG_E::Matrix{Float64}=zeros(length(ω), 6),      # [OPT2] Modal participation matrix = [Translation, Rotation]
+    )
+
+    # CoM position
+    posOG_O = posOE_O + R_OE*posEG_E
+
+    # Compute inertias
+    if any(inertiaE_E .!= 0.0)
+        verifyInertia(ID, inertiaE_E)
+        inertiaG_E .= translateInertia(inertiaE_E, -mass, posEG_E)
+    elseif any(inertiaG_E .!= 0.0)
+        verifyInertia(ID, inertiaG_E)
+        inertiaE_E .= translateInertia(inertiaG_E, +mass, posEG_E)
+    else
+        inertiaG_E .= elementInertiaG_E(geometry, mass)
+        inertiaE_E .= translateInertia(inertiaG_E, +mass, posEG_E)
+    end
+    inertiaG_O = rotateInertia(R_OE, inertiaG_E)
+    inertiaO_O = translateInertia(inertiaG_O, mass, posOG_O)
+
+    if isnan(ω[1])
+        # Return rigid element
+        return RigidElement(ID, geometry, mass, inertiaE_E, inertiaG_E, R_OE, posEG_E, posOE_O, posOG_O, inertiaO_O, inertiaG_O)
+    end
+
+    # Modal participation matrix
+    if any(LE_E .!= 0.0)
+        LG_E .= translateModalMatrix(LE_E, posEG_E)
+    else
+        LE_E .= translateModalMatrix(LG_E, -posEG_E)
+    end
+    LG_O = rotateModalMatrix(R_OE, LG_E)
+    LO_O = translateModalMatrix(LG_O, -posOG_O)
+
+    # Verify residual mass matrix
+    verifyResidualMass(ID, mass, inertiaG_O, LG_O)
+
+    # Sort things out
+    id = sortperm(ω)
+
+    # Return flexible element
+    return FlexibleElement(ID, geometry, mass, inertiaE_E, inertiaG_E, R_OE, posEG_E, posOE_O, posOG_O, inertiaO_O, inertiaG_O, LO_O[id, :], LG_O[id, :], ω[id], ξ[id])
+end
+
+
+#=
+# from O to Q (=new O)
+function transformElement!(el::SpacecraftElement, posQO_Q=zeros(3), R_QO=I)
+el.R_OE .= R_QO*el.R_OE
+el.posOE_O .= posQO_Q + R_QO*el.posOE_O
+el.posOG_O = posQO_Q + R_QO*el.posOG_O
+el.inertiaG_O = rotateInertia(R_QO, el.inertiaG_O)
+el.inertiaO_O = translateInertia(el.inertiaG_O, el.mass, el.posOG_O)
+if typeof(el) == FlexibleElement
+    el.LG_O = rotateModalMatrix(R_QO, el.LG_O)
+    el.LO_O = translateModalMatrix(el.LG_O, -el.posOG_O)
+end
+end
+
+function transformElement(el::SpacecraftElement, posQO_Q=zeros(3), R_QO=I)
+el2 = deepcopy(el)
+transformElement!(el2, posQO_Q, R_QO)
+return el2
+end
+=#
+
+function build(elements::Vector; plotModel=false)
+
+    if plotModel
+        set_theme!(theme_fra(true))
+        f = Figure(size=(1500, 1000))
+        p3d = LScene(f[1, 1]; show_axis=false)
+    end
+
+    ID = ""
+    mass = 0.0
+    posOG_O = zeros(3)
+    inertiaO_O = zeros(3, 3)
+    LO_O = zeros(1, 6)
+    ω = [NaN]
+    ξ = [NaN]
+    hasFlex = false
+    for el in elements
+        ID = ID*" + "*el.ID
+        mass += el.mass
+        posOG_O .+= el.mass*el.posOG_O
+        inertiaO_O .+= el.inertiaO_O
+
+        if typeof(el) == FlexibleElement
+            LO_O = [LO_O; copy(el.LO_O)]
+            ω = [ω; copy(el.ω)]
+            ξ = [ξ; copy(el.ξ)]
+            if !hasFlex
+                # Remove NaN values
+                LO_O = LO_O[2:end, :]
+                ω = ω[2:end]
+                ξ = ξ[2:end]
+                hasFlex = true
+            end
+        end
+
+        if plotModel
+            if typeof(el.geometry) == Cuboid
+                mesh!(p3d, cuboidModel(el.geometry.lx, el.geometry.ly, el.geometry.lz; pos_I=el.posOG_O, R_IB=el.R_OE); color=:lawngreen, alpha=0.5)
+            end
+            plotframe!(p3d, el.posOG_O, el.R_OE, 2.0)
+            scatter!(p3d, el.posOE_O[1], el.posOE_O[2], el.posOE_O[3]; markersize=10)   # This gets hidden by the mesh!
+        end
+    end
+    posOG_O ./= mass
+
+    if plotModel
+        plotframe!(p3d, posOG_O, Matrix(1.0I, 3, 3), 3.0)
+        display(f)
+    end
+
+    return initElement(ID=ID[4:end], mass=mass, inertiaE_E=inertiaO_O, posEG_E=posOG_O, ω=ω, ξ=ξ, LE_E=LO_O)
+end
+
+
+function buildss(sc::SpacecraftElement; attitudeOnly=false)
+    nω = length(sc.ω)
+    if attitudeOnly
+        nx = nω + 3
+        nu = 3
+        M = [sc.inertiaG_O sc.LG_O[:, 4:6]'; sc.LG_O[:, 4:6] I]
+    else
+        nx = nω + 6
+        nu = 6
+        M = [[sc.mass*I zeros(3, 3); zeros(3, 3) sc.inertiaG_O] sc.LG_O'; sc.LG_O I]
+    end
+    D = diagm([zeros(nx - nω); 2sc.ξ.*sc.ω])
+    K = diagm([zeros(nx - nω); sc.ω.^2])
+    Bu = [I; zeros(nω, nu)]
+    Cy = [I zeros(nx - nω, nω)]
+    A = [zeros(nx, nx) I; -M\K -M\D]
+    B = [zeros(nx, nu); M\Bu]
+    C = [Cy zeros(size(Cy))]
+    return ss(A, B, C, 0)
+end

src/scElements.jl

@@ -0,0 +1,84 @@
+
+abstract type SpacecraftElement end
+
+mutable struct RigidElement <: SpacecraftElement
+    ID::String
+    geometry::SpacecraftGeometry
+    mass::Float64                # Input
+    inertiaE_E::Matrix{Float64}  # Possible input
+    inertiaG_E::Matrix{Float64}  # Possible input
+    R_OE::Matrix{Float64}        # Input
+    posEG_E::Vector{Float64}     # Input
+    posOE_O::Vector{Float64}     # Input
+    posOG_O::Vector{Float64}     # Automatically computed
+    inertiaO_O::Matrix{Float64}  # Automatically computed
+    inertiaG_O::Matrix{Float64}  # Automatically computed
+end
+
+mutable struct FlexibleElement <: SpacecraftElement
+    ID::String
+    geometry::SpacecraftGeometry
+    mass::Float64                   # Input
+    inertiaE_E::Matrix{Float64}     # Possible input
+    inertiaG_E::Matrix{Float64}     # Possible input
+    R_OE::Matrix{Float64}           # Input
+    posEG_E::Vector{Float64}        # Input
+    posOE_O::Vector{Float64}        # Input
+    posOG_O::Vector{Float64}        # Automatically computed
+    inertiaO_O::Matrix{Float64}     # Automatically computed
+    inertiaG_O::Matrix{Float64}     # Automatically computed
+    LO_O::Matrix{Float64}           # Automatically computed
+    LG_O::Matrix{Float64}           # Automatically computed
+    ω::Vector{Float64}              # Input
+    ξ::Vector{Float64}              # Input
+end
+
+function RigidElement(;
+        ID::String="R-Element",
+        mass::Float64=0.0,
+        inertiaE_E::Matrix{Float64}=zeros(3, 3),        # [OPT1] Inertia
+        inertiaG_E::Matrix{Float64}=zeros(3, 3),        # [OPT2] Inertia
+        geometry::SpacecraftGeometry=NoGeometry(),
+        R_OE::Matrix{Float64}=Matrix(1.0I, 3, 3),
+        posEG_E::Vector{Float64}=zeros(3),
+        posOE_O::Vector{Float64}=zeros(3)
+    )
+
+    return initElement(ID=ID, geometry=geometry, mass=mass, inertiaE_E=inertiaE_E,
+        inertiaG_E=inertiaG_E, R_OE=R_OE, posEG_E=posEG_E, posOE_O=posOE_O)
+end
+
+function FlexibleElement(;
+        ID::String="F-Element",
+        mass::Float64=0.0,
+        inertiaE_E::Matrix{Float64}=zeros(3, 3),        # [OPT1] Inertia
+        inertiaG_E::Matrix{Float64}=zeros(3, 3),        # [OPT2] Inertia
+        geometry::SpacecraftGeometry=NoGeometry(),
+        R_OE::Matrix{Float64}=Matrix(1.0I, 3, 3),
+        posEG_E::Vector{Float64}=zeros(3),
+        posOE_O::Vector{Float64}=zeros(3),
+        ω::Vector{Float64}=[NaN],
+        ξ::Vector{Float64}=[NaN],
+        LE_E::Matrix{Float64}=zeros(length(ω), 6),      # [Translation, Rotation]
+        LG_E::Matrix{Float64}=zeros(length(ω), 6),      # [Translation, Rotation]
+    )
+
+    return initElement(ID=ID, geometry=geometry, mass=mass, inertiaE_E=inertiaE_E,
+        inertiaG_E=inertiaG_E, R_OE=R_OE, posEG_E=posEG_E, posOE_O=posOE_O, ω=ω, ξ=ξ,
+        LE_E=LE_E, LG_E=LG_E)
+end
+
+function Base.show(io::IO, obj::SpacecraftElement)
+    println("ID:         $(obj.ID)")
+    println("type:       $(typeof(obj))")
+    println("mass:       $(round(obj.mass, digits=2)) kg")
+    println("posOG_O:    $(round.(obj.posOG_O, digits=3)) m")
+    println("inertia at CoM:")
+    println("  Ixx, Iyy, Izz: $(round.(diag(obj.inertiaG_O), digits=1)) kg m²")
+    println("  Ixy, Ixz, Iyz: $(round.([obj.inertiaG_O[1, 2], obj.inertiaG_O[1, 3], obj.inertiaG_O[2, 3]], digits=1)) kg m²")
+    #if typeof(obj) == FlexibleElement
+    #    println("ω:          $(round.(obj.ω, digits=2)) rad/s")
+    #    println("ξ:          $(round.(obj.ξ*100, digits=2)) %")
+    #    println("LO_O:       $(round.(obj.LO_O, digits=2))")
+    #end
+end

src/utils.jl

@@ -0,0 +1,48 @@
+rotateInertia(R_BA, J_A) = R_BA*J_A*R_BA'  # J_B
+rotateModalMatrix(R_BA, L_A) = L_A*[R_BA' zeros(3, 3); zeros(3, 3) R_BA']         # L_B
+
+# posGA = position of point A wrt element CoM (G).
+# JG  = inertia at element CoM.
+# posGA and JG must be written in the same reference frame (X).
+#
+# To get JG from JA, provide a negative mass!
+# JG = translateInertia(JA,-mass,posGA)
+translateInertia(JG_X, mass, posGA_X) = JG_X - mass*crossmat(posGA_X)^2    # JA_X
+translateModalMatrix(LA_X, posAB_X) = LA_X*[I crossmat(posAB_X); zeros(3, 3) I]     # LB_X
+
+function verifyInertia(ID, J)
+    if maximum(abs.(J - J')) > 1e-8
+        println("Warning: the inertia matrix of $ID is not symmetric")
+        return
+    end
+
+    eigsJ = eigvals(J)
+    if any(.!isreal(eigsJ)) || any(eigsJ .< 0.0)
+        println("Warning: the inertia matrix of $ID is not definite positive")
+        return
+    end
+
+    if !(J[1, 1] ≤ J[2, 2] + J[3, 3] && J[2, 2] ≤ J[1, 1] + J[3, 3] && J[3, 3] ≤ J[1, 1] + J[2, 2])
+        println("Warning: the diagonal terms of the inertia matrix of $ID are not physical")
+        return
+    end
+
+    if !(abs(J[1, 2]) <= J[3, 3]/2 && abs(J[1, 3]) <= J[2, 2]/2 && abs(J[2, 3]) <= J[1, 1]/2)
+        println("Warning: the non-diagonal terms of the inertia matrix of $ID are not physical")
+        return
+    end
+end
+
+function verifyResidualMass(ID, m, J, L)
+    MR = [m*I zeros(3, 3); zeros(3, 3) J] - L'*L
+    if maximum(abs.(MR - MR')) > 1e-8
+        println("Warning: the residual mass matrix of $ID is not symmetric")
+        return
+    end
+
+    eigsJ = eigvals(MR)
+    if any(.!isreal(eigsJ)) || any(eigsJ .< 0.0)
+        println("Warning: the residual mass matrix of $ID is not definite positive")
+        return
+    end
+end