AbstractFunction API

AbstractFunction

A function of the form:

where $x$ is a state vector of size n and $u$ is a control input of size m for some dynamical system. The output $y$ is of size p. Here $t$ represents any number of optional extra parameters to the function (such as time, time step, etc.).

The dimensions n, m, and p can be queried individual by calling state_dim, control_dim, and ouput_dim or collectively via dims.

Evaluation

An AbstractFunction can be evaluated in-place by calling any of

evaluate!(fun, y, z::AbstractKnotPoint)
evaluate!(fun, y, x, u, p)
evaluate!(fun, y, x, u)

The top method should be preferred by generic APIs.

The function can also be evaluated out-of-place (generally returning a StaticArrays.SVector) by calling any of

evaluate(fun, z::AbstractKnotPoint)
evaluate(fun, x, u, p)
evaluate(fun, x, u)

Alternatively, the user can dispatch on FunctionSignature by calling

evaluate!(::FunctionSignature, fun, y, z::AbstractKnotPoint)

Jacobians

The Jacobian of $f(x,u)$ with respect to both $x$ and $u$ can be computed by calling the following function:

jacobian!(::FunctionSignature, ::DiffMethod, fun, J, y, z::AbstractKnotPoint)

Where J is the Jacobian of size (p, n + m) and y is provided as an extra storage vector for evaluating the Jacobian. Some users may whish to store the function output value in this vector if evaluating the Jacobian and output value together offers some computational savings, but this behavior is left to the user to implement on their particular function.

A user-defined Jacobian, called by passing the UserDefinedDiffMethod, can be implemented for the function by defining any of the following methods for the AbstractFunction:

jacobian!(fun, J, y, z::AbstractKnotPoint)
jacobian!(fun, J, y, x, u, p)
jacobian!(fun, J, y, x, u)

Functions of just the state or control

Alternatively, the function can also be limited to an input of just the state or control by defining the FunctionInputs trait. See trait documentation for more information.

Convenience functions

The methods fill(fun, v), randn(fun), rand(fun), and zeros(fun) are defined that provide a tuple with a state and control vector initialized using the corresponding function. The data type can be specified as the first argument for randn, rand, and zeros.

dims(fun::AbstractFunction)

Get the tuple (n,m,p) where n is the dimension of the state vector, m is the dimension of the control vector, and p is the output dimension of the function.

input_dim(fun::AbstractFunction)

Get the dimension of the inputs to the function. Will be equal to n + m for a StateVector function, n for a StateOnly function, and m for a ControlOnly function.

FunctionSignature

Specifies which function signature to call when evaluating the function, must be either StaticReturn or InPlace. The default signature for a function can be queried via default_signature(fun).

DiffMethod

Represents the method used to evaluate the Jacobian of the function. Allows the user to implement multiple methods and switch between them as needed. The following methods are provided:

• ForwardAD: forward-mode automatic differentiation using ForwardDiff.jl
• FiniteDifference: finite difference approximation using FiniteDiff.jl
• UserDefined: analytical function defined by the user.
• ImplicitFunctionTheorem: Only available for a DiscretizedDynamics model with an an implicit integrator. Parameterized by the DiffMethod of the underlying continuous dynamics model.

The UserDefined method can be provided by defining any of the following methods on your AbstractFunction:

jacobian!(fun, J, y, z::AbstractKnotPoint)
jacobian!(fun, J, y, x, u, p)
jacobian!(fun, J, y, x, u)

The other two modes must be explicitly defined on your function, since some functions may wish to tailor the method to their particular function. Efficient default implementations can be defined automatically via the @autodiff macro.

StateVectorType

A trait defined on an AbstractFunction, describing how state vectors are composed and how the error state, or the "difference" between two vectors, is computed. These "error states" should be Euclidean vectors, and can have a different dimension than the original state vectors.

The following types are provided by RobotDynamics:

• EuclideanState
• RotationState

Defining a new state vector type

To define a custom state vector type (e.g. NewStateType), you need to implement the function that calculates the difference between state vectors, as well the "error state Jacobian" and it's derivative. First, you must specify the dimension of the error state by defining

errstate_dim(::NewStateType, fun::AbstractFunction)

Then you define method that calculates the error state between two state vectors:

state_diff!(::NewStateType, model, dx, x, x0)

where dx should have dimension e = output_dim(model) and x and x0 should have dimension n = state_dim(model).

The other piece of information we need is the "error state Jacobian." This is the first-order approximation of the mapping from local error state values to the true state vector. Let x ⊗ x0 be the composition of two state vectors, and let φ be the function that maps a Euclidean error state δx to a state vector. The error state Jacobian is the Jacobian of x ⊗ φ(δx) with respect to δx, taking the limit as δx → 0. The resulting function is solely a function of the state x, and is implemented with the following function:

errstate_jacobian!(::NewStateType, model, G, x)

where G has dimensions (n,e).

We'll also need the derivative of this function. For computational efficiency, we compute the Jacobian of the error state Jacobian transposed with some vector x̄, where x̄ is state vector of size (n,). More precisely, it is the Hessian of

math \bar{x}^T (x \otimes \varphi(\delta x))` This function is calculated using

∇errstatef_jacobian!(::NewStateType, model, ∇G, x, xbar)

where ∇G is of size (e,e).

EuclideanState

The space of standard Euclidean vectors, where composition is element-wise addition, and the error state is just element-wise subtraction.

RotationState

The space of vectors composes of Euclidean states intermixed with 3D rotations, represented using any 3 or 4-parameter representation (usually unit quaternions). The error state Jacobians and error state calculation are all calculating using Rotations.jl.

statevectortype

Queries the StateVectorType trait on a function.

FunctionInputs

A trait of an AbstractFunction that specified the inputs to the function. By default, the input is assumed to be both the state and the control vector, along with any extra parameters. This trait allows the user to change this assumption, defining functions on just the state or control vectors. The number of columns in the Jacobian will change accordingly, and can be queried via input_dim. Functions of just the state or control are not allowed to take in any extra parameters as arguments.

This trait is queried via functioninputs(fun).

The following three options are provided:

• StateControl: a function of the form $y = f(x,u)$ (default)
• StateOnly: a function of the form $y = f(x)$
• ControlOnly: a function of the form $y = f(u)$

When defining methods for these functions, you need to disambiguate methods of the form

evaluate(fun::AbstractFunction, z::AbstractKnotPoint)

from

evaluate(fun::MyStateOnlyFunction, x)

It's not enough to annotate x as an AbstractVector because AbstractKnotPoint <: AbstractVector. For most cases, the RobotDynamics.DataVector type should be sufficient to accomplish this. For StateOnly or ControlOnly functions, your methods should look like:

evaluate(fun::MyFunction, x::RobotDynamics.DataVector)
evaluate!(fun::MyFunction, y, x::RobotDynamics.DataVector)
jacobian!(fun::MyFunction, J, y, x::RobotDynamics.DataVector)

functioninputs(fun::AbstractFunction)

Get the FunctionInputs trait on the function, specifying whether the function takes as input both state and control vectors (default), or just the state or control vector independently.