Linear Models

RobotDynamics supports the easy construction of linear models. By defining a linear model, the relevant dynamics and jacobian functions are predefined for you. This can result in signicant speed ups compared to a naive specification of a standard continuous model.

RobotDynamics.LinearModel — Type

LinearModel{n,m,T} <: AbstractModel

A concrete type for creating efficient linear model representations. This model type will automatically define the continuous or discrete version of the dynamics and jacobian functions. Supports continuous/discrete, time invariant/varying, and affine models.

Constructors

LinearModel(A::AbstractMatrix, B::AbstractMatrix, [dt=0, use_static]) # time invariant
LinearModel(A::AbstractMatrix, B::AbstractMatrix, d::AbstractVector, [dt=0, use_static]) # time invariant affine
LinearModel(A::Vector{TA}, B::Vector{TB}, [times::AbstractVector, dt::Real=0, use_static]) # time varying
LinearModel(A::Vector{TA}, B::Vector{TB}, d::Vector{Td}, [times::AbstractVector, dt=0, use_static]) # time varying affine

LinearModel(n::Integer, m::Integer, [is_affine=false, times=1:0, dt=0, use_static]) # constructor with zero dynamics matrices

By default, the model is assumed to be continuous unless a non-zero dt is specified. For time varying models, searchsortedlast is called on the times vector to get the discrete time index from the continuous time. The use_static keyword is automatically specified based on array size, but can be turned off in case of excessive compilation times.

source

RobotDynamics.LinearizedModel — Type

LinearizedModel{M,L,T} <: AbstractLinearModel

A container for the linearized model that holds the full nonlinear model, the linearized model, and the trajectory of linearization points. The same dynamics and jacobian functions can still be called on the LinearizedModel type.

Constructors

LinearizedModel(nonlinear_model::AbstractModel, Z::AbstractTrajectory; kwargs...)
LinearizedModel(nonlinear_model::AbstractModel, [z::AbstractKnotPoint]; kwargs...)

Linearizes nonlinear_model about the trajectory Z or a single point z. If not specified, z is defined as the state and control defined by zeros(nonlinear_model).

Linearization is, by default, on the continuous system.

Keyword Arguments

is_affine - Linearize the system with an affine term, such that the new state is the same as the original state. See below for more details.
dt - Time step. If not provided, defaults to the value in Z or z. Must be specified and non-zero for the system to be discretized. If dt = NaN, then the dt will be inferred from the trajectory (useful for variable step sizes).
integration - An explicit integration method. Must also specify a non-zero dt.

If is_affine = false, the dynamics are defined as: $f(x,u) \approx f(x_0, u_0) + A \delta x + B \delta u$

which defines the error state $\delta x = x - x_0$ as the state of the linearized system. Here $A$ and $B$ are the partial derivative of the dynamics with respect to the state and control, respectively.

If is_affine = true, the form is an affine function of the form $f(x,u) \approx A x + B u + d$

where $d = f(x_0,u_0) - A x_0 - B u_0$, which maintains the same definition of the state.

source

Linearizing and Discretizing a Model (experimental)

Many systems with complicated nonlinear dynamics can be simplified by linearizing them about a fixed point or a trajectory. This can allow the use of specialized and faster trajectory optimization methods for these linear systems. The functions that RobotDynamics provides also discretize the system.

RobotDynamics.discretize! — Function

discretize!(::Type{Q}, model::LinearizedModel, k)

Discretize the linearized model at time step k, using integration Q.

source

RobotDynamics.update_trajectory! — Function

update_trajectory!(model::LinearizedModel, Z::AbstractTrajectory, integration::=DEFAULT_Q)

Updates the trajectory inside of the model and relinearizes (and discretizes for discrete models) the model about the new trajectory.

source

Example

Take for example the cartpole, which can be readily linearized about it's stable point. We can use the LinearizedModel to easily find the linearized system.

using RobotDynamics
import RobotDynamics: dynamics  # the dynamics function must be imported
using StaticArrays

const RD = RobotDynamics

struct Cartpole{T} <: AbstractModel
    mc::T  # mass of the cart
    mp::T  # mass of the pole
    l::T   # length of the pole
    g::T   # gravity
end

function dynamics(model::Cartpole, x, u)
    mc = model.mc   # mass of the cart in kg (10)
    mp = model.mp   # mass of the pole j(point mass at the end) in kg
    l = model.l     # length of the pole in m
    g = model.g     # gravity m/s^2

    q  = x[SA[1,2]]
    qd = x[SA[3,4]]

    s = sin(q[2])
    c = cos(q[2])

    H = SA[mc+mp mp*l*c; mp*l*c mp*l^2]
    C = SA[0 -mp*qd[2]*l*s; 0 0]
    G = SA[0, mp*g*l*s]
    B = SA[1, 0]

    qdd = -H\(C*qd + G - B*u[1])
    return [qd; qdd]
end

RD.state_dim(::Cartpole) = 4
RD.control_dim(::Cartpole) = 1

nonlinear_model = Cartpole(1.0, 1.0, 1.0, 9.81)
n = state_dim(nonlinear_model)
m = control_dim(nonlinear_model)

# stationary point for the cartpole around which to linearize
x̄ = @SVector [0., π, 0., 0.]
ū = @SVector [0.0]
dt = 0.01
knot_point = KnotPoint(x̄, ū, dt)

# creates a new LinearizedModel around stationary point
linear_model = RD.LinearizedModel(nonlinear_model, knot_point, 
    dt=dt, integration=Exponential)

δx = @SVector zeros(n)
δu = @SVector zeros(m)

# outputs linearized dynamics!
δxₖ₊₁ = discrete_dynamics(PassThrough, linear_model, δx, δu, 0.0, dt) 

@assert δxₖ₊₁ ≈ zeros(n)

F = RD.DynamicsJacobian(n,m)
discrete_jacobian!(PassThrough, F, linear_model, knot_point)

@show A = RD.get_A(F)
@show B = RD.get_B(F)