Skip to content

A general purpose numerical simulator supporting nested dynamical systems and a convenient macro-based data logger.

License

Notifications You must be signed in to change notification settings

JinraeKim/FlightSims.jl

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

FlightSims

FlightSims.jl is a general-purpose numerical simulator supporting nested environments and convenient macro-based data logging.

APIs

Main APIs can be found in FSimBase.jl. In FlightSims.jl, the default differential equation (DE) solver is Tsit5() for ordinary DE (ODE).

Features

If you want more functionality, please feel free to report an issue!

Nested Environments and Zoo

  • Environments usually stand for dynamical systems but also include other utilities, for example, controllers.
  • One can generate user-defined nested environments using provided APIs. Also, some predefined environments are provided for reusability. Take a look at FSimZoo.jl.

Utilities

  • Some utilities are also provided for dynamical system simulation.
  • Examples include
    • Simulation rendering
      • See FSimPlots.jl. Note that FSimPlots.jl is not exported in the default setting to reduce precompilation time.
    • ROS2 compatibility
      • See FSimROS.jl. Note that FSimROS.jl is not exported in the default setting.

Examples

Basic

Minimal examples

  • For minimal examples of FlightSims.jl, see FSimBase.jl.

Discrete problem

  • See test/environments/basics/discrete_problem.jl.
using DifferentialEquations


function main()
    t0, tf = 0, 10
    x0 = [1.0, 2, 3]
    """
    dx: next x
    """
    @Loggable function dynamics!(dx, x, p, t; u)
        @log x
        dx .= 0.99*x + u
        @onlylog u_next = dx
    end
    simulator = Simulator(x0, apply_inputs(dynamics!; u=zeros(3));
                          Problem=:Discrete,
                          tf=tf,  # default step length = 1 for Discrete problem
                         )
    df = solve(simulator)
end
julia> main()
11×2 DataFrame
 Row │ time     sol
     │ Float64  NamedTup
─────┼────────────────────────────────────────────
   10.0  (u_next = [0.99, 1.98, 2.97], x 
   21.0  (u_next = [0.9801, 1.9602, 2.940
   32.0  (u_next = [0.970299, 1.9406, 2.9
   43.0  (u_next = [0.960596, 1.92119, 2.
   54.0  (u_next = [0.95099, 1.90198, 2.8
   65.0  (u_next = [0.94148, 1.88296, 2.8
   76.0  (u_next = [0.932065, 1.86413, 2.
   87.0  (u_next = [0.922745, 1.84549, 2.
   98.0  (u_next = [0.913517, 1.82703, 2.
  109.0  (u_next = [0.904382, 1.80876, 2.
  1110.0  (u_next = [0.895338, 1.79068, 2.

Dynamical system control

Optimal control and reinforcement learning

  • For an example of infinite-horizon continuous-time linear quadratic regulator (LQR) with callbacks, see the following example code (test/environments/basics/lqr.jl).
using FlightSims
const FS = FlightSims
using DifferentialEquations
using LinearAlgebra
using Plots
using Test


function main()
    # linear system
    A = [0 1;
         0 0]  # 2 x 2
    B = [0 1]'  # 2 x 1
    n, m = 2, 1
    env = LinearSystem(A, B)  # exported from FlightSims
    x0 = State(env)([0.5, 0.5])
    # optimal control
    Q = Matrix(I, n, n)
    R = 10.0*Matrix(I, m, m)
    lqr = LQR(A, B, Q, R)  # exported from FlightSims
    u_lqr = Command(lqr)  # (x, p, t) -> -K*x; minimise J = ∫ (x' Q x + u' R u) from 0 to ∞
    u0 = u_lqr(x0)
    p0 = zeros(size(u0)...)  # auxiliary parameter

    # simulation
    Δt = 0.05
    tf = 10.0
    affect!(integrator) = integrator.p .= u_lqr(copy(integrator.u))  # auxiliary callback funciton
    cb = PeriodicCallback(affect!, Δt; initial_affect=true)  # auxiliary callback
    @Loggable function dynamics!(dx, x, p, t)
        @onlylog p  # activate this line only when logging data
        u = p
        @log x, u
        @nested_log Dynamics!(env)(dx, x, p, t; u=u)  # exported `state` and `input` from `Dynamics!(env)`
    end
    simulator = Simulator(x0, dynamics!, p0;
                          tf=tf)
    df = solve(
               simulator;
               callback=cb, savestep=Δt,
               dtmax=Δt/2,
              )
    # CAUTION: when using PeriodicCallback for MPC-like control, the initial input may be overwritten by the second input.
    # I guess that the adaptive solver "goes back in time" when `dt` is not set small enough, which violates the results with Callbacks including SavingCallback and PeriodicCallback.
    ts = df.time
    xs = [datum.x for datum in df.sol]
    us = [datum.u for datum in df.sol]
    ps = [datum.p for datum in df.sol]
    states = [datum.state for datum in df.sol]
    inputs = [datum.input for datum in df.sol]
    @test u0 == us[1]
    @test xs == states
    @test us == inputs
    p_x = plot(ts, hcat(states...)';
               title="state variable", label=["x1" "x2"], color=[:black :black], lw=1.5,
              )  # Plots
    plot!(p_x, ts, hcat(ps...)';
          ls=:dash, label="param", color=[:red :orange], lw=1.5
         )
    p_u = plot(ts, hcat(inputs...)'; title="control input", label="u", seriestype=:steppost)  # Plots
    fig = plot(p_x, p_u; layout=(2, 1))
    mkpath("figures")
    savefig(fig, "figures/lqr.png")
    display(fig)
    df
end

@testset "lqr example" begin
    main()
end
julia> main()
1001×2 DataFrame
  Row │ time     sol
      │ Float64  NamedTup
──────┼────────────────────────────────────────────
    10.0   (p = [1.01978, 1.95564], state =
    20.01  (p = [1.01978, 1.95564], state =
    30.02  (p = [1.03911, 1.91186], state =
    40.03  (p = [1.05802, 1.86863], state =
    50.04  (p = [1.07649, 1.82596], state =
                       
  9989.97  (p = [-0.00093419, 0.00103198], 
  9999.98  (p = [-0.000923913, 0.00102347],
 10009.99  (p = [-0.00091372, 0.001015], st
 100110.0   (p = [-0.00091372, 0.001015], st
                                   992 rows omitted

ex_screenshot

Linear system with zero-order-hold (ZOH) input

  • Note that this example utilises interactive simulation interface. See test/environments/integrated_environments/linear_system_zoh_input.jl.
using FlightSims
import FlightSims: State, Dynamics!
using DataFrames
using ComponentArrays
using UnPack
using Transducers
using Plots
using SciMLBase
using Test


struct LinearSystem_ZOH_Input <: AbstractEnv
    linear_env::LinearSystem
end

function State(env::LinearSystem_ZOH_Input)
    State(env.linear_env)
end

function Dynamics!(env::LinearSystem_ZOH_Input)
    @Loggable function dynamics!(dx, x, input, t)
        @nested_log Dynamics!(env.linear_env)(dx, x, nothing, t; u=input)
    end
end

function main()
    A = [0 1;
         0 0]
    B = [0 1]'
    linear_env = LinearSystem(A, B)
    env = LinearSystem_ZOH_Input(linear_env)
    x0_state = [1, 2]
    input = zeros(1)
    x0 = State(env)(x0_state)
    t0 = 0.0
    tf = 20.0
    simulator = Simulator(x0, Dynamics!(env), input; tf=tf)
    # interactive sim
    Δt = 0.1  # save period
    df = DataFrame()
    @time for (i, t) in enumerate(t0:Δt:tf)
        # To perform interactive simulation,
        # you should be aware of the integrator interface
        # provided by DifferentialEquations.jl;
        # see https://diffeq.sciml.ai/stable/basics/integrator/#integrator
        # e.g., DO NOT directly change the integrator state
        state = simulator.integrator.u
        input = simulator.integrator.p
        if (i-1) % 10 == 0  # update input period
            input .= -sum(state)
        end
        step_until!(simulator, t, df)
    end
    # plot
    ts = df.time
    states = df.sol |> Map(datum -> datum.state) |> collect
    inputs = df.sol |> Map(datum -> datum.input) |> collect
    p_x = plot(ts, hcat(states...)';
               title="state variable", label=["x1" "x2"],
              )
    p_u = plot(ts, hcat(inputs...)';
               title="input variable", label="u",
               linetype=:steppost,  # to plot zero-order-hold input appropriately
              )
    fig = plot(p_x, p_u; layout=(2, 1))
    savefig("figures/interactive_sim.png")
    display(fig)
end


@testset "linear_system_zoh_input" begin
    main()
end

ex_screenshot

Multicopter position control

  • For an example of backstepping position tracking controller for quadcopters, see test/environments/integrated_environments/backstepping_position_controller_static_allocator_multicopter_env.jl. See ./test/environments/controllers/geometric_tracking.jl and ./test/environments/controllers/geometric_tracking_inner_outer.jl for the geometric tracking controller.

ex_screenshot

Control barrier function (CBF) methods

For examples of CBF methods, see ./test/environments/controllers/cbf.jl.

Visualisation

Missile guidance with interactive visualisation

  • See test/pluto_guidance.jl (thanks to @nhcho91).

Alt Text

Multicopter rendering

Alt Text

Examples with ROS2

See FSimROS.jl.

Alt Text

Related packages

FSim family

  • FSimBase.jl is the lightweight base package for numerical simulation supporting nested dynamical systems and macro-based data logger. For more functionality, see FlightSims.jl.
  • FSimZoo.jl contains predefined environments and controllers for FlightSims.jl.
  • FSimPlots.jl is the plotting package for predefined environments exported from FlightSims.jl
  • FSimROS.jl is a package of FlightSims.jl family for ROS2.

Packages using FlightSims.jl

  • FaultTolerantControl.jl: fault tolerant control (FTC) with various models and algorithms of faults, fault detection and isolation (FDI), and reconfiguration (R) control.
  • FlightGNC.jl (@nhcho91): FlightGNC.jl is a Julia package containing GNC algorithms for autonomous systems.

Useful packages

Trouble shootings

solve produces an empty Dataframe

  • Please check whether you put @Loggable in front of the dynamics function in a proper way, e.g.,
function Dynamics!(env::MyEnv)
    @Loggable function dynamics!(dx, x, p, t; u)
    # return @Loggable dynamics!(dx, x, p, t; u)  # This would not work
        # blahblah...
    end
end

@Loggable does not work; ERROR: LoadError: LoadError: LoadError: LoadError: LoadError: LoadError: KeyError: key :name not found

  • Please check whether you assigned a name of function annotated by @Loggable, e.g.,
function Dynamics!(env::YourEnv)
    @Loggable function dynamics!(dX, X, p, ; u)
        ...
    end
end

instead of

function Dynamics!(env::YourEnv)
    @Loggable function (dX, X, p, ; u)
        ...
    end
end

The first input of the logged inputs is incorrect when using PeriodicCallback for zero-order-hold (ZOH) control (#161)

Due to the adaptive-time-step solver, it might "go back in time", which contradicts the saved results from SavingCallback with the applied input using PeriodicCallback. If the time step of PeriodicCallback is Δt, then set the keyword argument dtmax of solve to be small, e.g., dtmax = Δt/2 (see ./test/environments/basics/lqr.jl as an example.)