Discrete Dynamics

DiscreteDynamics

A dynamics model of the form

where $x$ is the n-dimensional state vector and $u$ is the m-dimensional control vector. The following methods should be defined on any model:

state_dim(model)
control_dim(model)

The output dimension output_dim is automatically defined to be equal to the state dimension.

Defining the dynamics

To define the dynamics using out-of-place (returning an SVector) evaluation, the user must define one of the following:

discrete_dynamics(model, z::AbstractKnotPoint)
discrete_dynamics(model, x, u, t, dt)

where t is the current time and dt is the time step between $x_k$ and $x_{k+1}$.

The user shouldn't assume that the inputs are static arrays, although for best computational performance these methods should be called by passing in static arrays.

To define the dynamics using in-place evaluation, the user must define one of the following:

discrete_dynamics!(model, xn, z::AbstractKnotPoint)
discrete_dynamics!(model, xn, x, u, t, dt)

Either of these function can be called by dispatching on the FunctionSignature using:

discrete_dynamics!(sig, model, xn, z::AbstractKnotPoint)

Discretizing continuous dynamics

This type is the abstract type representing all discrete dynamics systems, whether or not they are approximations of continuous systems. Users can choose to implement their dynamics directly in discrete time using this interface, or can use DiscretizedDynamics to discretize a continuous time system.

Implicit dynamics functions

Sometimes continuous time systems are approximated using implicit integrators of the form

To evaluate these types of systems, use the dynamics_error and dynamics_error! methods, which will return the output of f for two consecutive knot points. The default functions such as discrete_dynamics or jacobian! are not defined for these types of systems.

Simulating the dynamics

Use propagate_dynamics! to conveniently save the next state directly into the state vector of another KnotPoint.

discrete_dynamics(model, z::AbstractKnotPoint)
discrete_dynamics(model, x, u, t, dt)

Evaluate the discrete time dynamics, returning the output $x_{k+1}$. For best performance, the output should be a StaticArrays.SVector. This method is called when using the StaticReturnFunctionSignature.

Calling evaluate on a DiscreteDynamics model will call this function.

discrete_dynamics!(model, xn, z::AbstractKnotPoint)
discrete_dynamics!(model, xn, x, u, t, dt)

Evaluate the discrete time dynamics, storing the output in xn. This method is called when using the InPlaceFunctionSignature.

Calling evaluate! on a DiscreteDynamics model will call this function.

discrete_dynamics!(sig, xn, z::AbstractKnotPoint)

Evaluate the discrete dynamics function, storing the output in xn, using the FunctionSignaturesig to determine which method to call.

dynamics_error(model, z2, z1)

Evaluate the dynamics error between two knot points for a DiscreteDynamics model. In general, this function takes the form:

For explicit integration methods of the form $x_{k+1} = f(x_k, u_k)$, this is just

This is the method that should be used with implicit integration methods.

This form is the out-of-place form that should, in general, return a StaticVectors.SVector. See dynamics_error! for the in-place method.

The Jacobian of this function is evaluated by calling dynamics_error_jacobian!.

dynamics_error!(model, y2, y1, z2, z1)

Evaluate the dynamics error between two knot points for a DiscreteDynamics model. The output is stored in y2, and y1 is provided as a extra input that can be used to store temporary results. Any specific usage of this variable is left to the user.

See dynamics_error for more details on this function.

dynamics_error_jacobian!(sig, diff, model, J2, J1, y2, y1, z2, z1)

Evaluate the Jacobian of dynamics_error. The derivative with respect to the first knotpoint z1 should be stored in J1, and the derivative with respect to the second knotpoint z2 should be stored in J2. The variables y2 and y1 should be vectors of the size of the output dimension (usually the state dimension) and are provided as extra cache variables whose usage is left to be determined by the user. For example, a user could choose to evaluate the dynamics error and store the result in one of these variables to evaluate the error and it's Jacobian at the same time.

Both J2 and J1 must have dimensions (p, n + m) where n, m, and p are the output of state_dim, control_dim, and output_dim. Usually p = n.

Note that for explicit integration methods of the form x_{k+1} = f(x_k, u_k)J2 should be equal to [-I(n) zeros(n,m)].

Implementing on a custom type

To implement this function on a new DiscreteDynamics model, define methods for the FunctionSignature and DiffMethod of your choice. These are automatically defined when using @autodiff on a DiscreteDynamics model. To implement the UserDefinedDiffMethod, implement the following method:

dynamics_error_jacobian!(model, J2, J1, y2, y1, 
                         z2::AbstractKnotPoint, z1::AbstractKnotPoint)

propagate_dynamics!(sig, model, z2, z1)

Save the output of either discrete_dynamics or discrete_dynamics! evaluated using z1 into the state vector of z2. Useful for simulating discrete systems forward in time.

DiscretizedDynamics

Represents a DiscreteDynamics model formed by integrating a continuous dynamics model. It is essentially a ContinuousDynamics paired with a QuadratureRule that defines how to use the dynamics! function to get a discrete_dynamics! function.

Constructor

A DiscretedDynamics type can be created using either of the following signatures:

DiscretizedDynamics(dynamics::ContinuousDynamics, Q::QuadratureRule)
DiscretizedDynamics{Q}(dynamics::ContinuousDynamics) where Q <: QuadratureRule

In the second case, the integrator is constructed by calling Q(state_dim(dynamics), control_dim(dynamics)).

Usage

A DiscretizedDynamics model is used just like any other DiscreteDynamics model. The state, control, and error state dimensions are all taken from the continuous time dynamics model, and the StateVectorType and FunctionInputs traits are inherited from the continuous time dynamics.

The default_diffmethod, however, is set to ForwardAD, since our benchmarks show that it is usually faster to make a single call to ForwardDiff.jacobian! than using the chain rule and multiple calls to query the Jacobian of the continuous dynamics at multiple points. If the continuous model has a UserDefined Jacobian method, calling jacobian! on a DiscretizedDynamics model will use the chain rule with analytical Jacobians of the integrator. If the user wants to use a combination of ForwardDiff (or any other differentiation method, for that matter) with the integrator, they are free to define their own DiffMethod to dispatch on.

Implicit Dynamics

If an Implicit integrator is used, the interface changes slightly. The dynamics residual is calculated and it's Jacobian are calculated using

dynamics_error(model, z2, z1)
dynamics_error!(model, y2, y1, z2, z1)
dynamics_error_jacobian!(sig, diff, model, J2, J1, y2, y1, z2, z1)

Where the dynamics residual is stored in y2 for the in-place version, and the Jacobians with respect to the state and control at the next and current knot points are stored in J2 and J1, respectively.

Implicit dynamics integrators can also support the normal API (see documentation for Implicit). Note that calls to discrete_dynamics will use a Newton's solve to calculate the next state and calls to jacobian! will use the Implicit Function Theorem. In order to use the implicit function theorem to get the Jacobians, jacobian! must be called using the ImplicitFunctionTheoremDiffMethod, which also specifies the DiffMethod to use to on dynamics_error_jacobian!.

When using Newton's method during discrete_dynamics, the DiffMethod returned by default_diffmethod for the continuous dynamics model is used to evaluate the dynamics_error_jacobian!. There is no option to change this at run time, since discrete_dynamics and discrete_dynamics! do not take a DiffMethod as an argument.

The convergence tolerance and max number of iterations of the Newton's method can be set using

setnewtontolerance(model::ImplicitDynamicsModel, tol)
setnewtoniters(model::ImplicitDynamicsModel, iters)

QuadratureRule

Integration rule for approximating the continuous integrals for the equations of motion. Currently divided into two classes of integrators:

• Explicit
• Implicit

Interface

All integrators need to have a constructor of the form:

Q(n, m)

where n and m are the state and control dimensions of the dynamical system.

Explicit <: QuadratureRule

Integration rules of the form

.

Interface

All explicit integrators need to define the following methods:

integrate(::Q, model, x, u, t, h)
integrate!(::Q, model, xn, x, u, t, h)
jacobian!(::Q, sig, model, J, xn, x, u, t, h)

where h is the time step (i.e. dt). The Jacobian J should have size (n, n + m). The sig is a FunctionSignature that can be used to dispatch the Jacobian on the desired function signature. For example, when using StaticReturn intermediate cache variables aren't usually needed, but are when using InPlace methods. These cache variables can be stored in the integrator itself (see the ADVector type provided by this package).

Implicit <: QuadratureRule

Integration rules of the form

.

Interface

All implicit integrators need to define the following methods:

dynamics_error(::Q, model, z2, z1)
dynamics_error!(::Q, model, y2, y1, z2, z1)
dynamics_error_jacobian!(::Q, sig, model, J2, J1, y2, y1, z2, z1)

where model is a ContinuousDynamics model. For the in place method, the output should be stored in the y2 vector. For the Jacobian method, J2 holds the Jacobian with respect to the state and control of z2 (the knot point at the next time step) and J1 holds the Jacobian with respect to the state and control of z1 (the knot point at the current time step).

Treating an Implicit method as an Explicit method

Implicit methods can be treated as an explicit integrator. In addition to the the methods above that evaluate the dynamics error residual for the integrator, they overload discrete_dynamics, discrete_dynamics!, and jacobian. The forward simulation functions discrete_dynamics and discrete_dynamics! use Newton's method. The dynamics Jacobians are calculated using the Implicit Function Theorem.

If the state and control vector passed to jacobian! are the same as the last call to jacobian!, discrete_dynamics or discrete_dynamics! the Jacobians and matrix factorization from the Newton solve are used to speed up the computation. Note that the factorization is only cached for InPlace methods, not StaticReturn.

To call jacobian! on DiscretizedDynamics with an implicit dynamics model, it must be called using the ImplicitFunctionTheorem method, which also specified the DiffMethod that should be used to evaluate dynamics_error_jacobian!, e.g.

jacobian!(InPlace(), ImplicitFunctionTheorem(ForwardDiff), implicitmodel, J, y, z)

Defining an Implicit Integrator

An implicit integrator should define the signatures above for dynamics_error and dynamics_error_jacobian!. If the method wants to provide the functionality of an explicit integrator (as described in the previous section), it should store internally a ImplicitNewtonCache, which must be returned using the getter function getnewtoncache.

Euler

Explicit Euler integration:

where $h$ is the time step.

RK3

A third-order explicit Runge-Kutta method:

RK4

The classic fourth-order explicit Runge-Kutta method.

ImplicitMidpoint

A symplectic method with second-order accuracy. A great option for those wanting good performance with few calls to the dynamics.

math x_1 + h f(\frac{1}{2}(x_1 + x_2), u_1, t + \frac{1}{2} h) - x_2 = 0`

ADVector{T}

A cache of vectors for making it easier to ForwardDiff. A vector of the desired type is extracted by indexing:

vec = adv[eltype(x)]

Since ForwardDiff Duals are parameterized on the chunk size, this type will store any number of vectors for different chunk sizes. A new vector of duals will be automatically created (once) if it doesn't exist yet.

For best performance (by avoiding type-instability), annotate the indexing operation:

vec = adv[T]::Vector{T}

While the annotation could have been included in the indexing operation itself, some benchmark results found it significantly faster to apply the annotation at the call site.

Constructor

ADVector{T}(n)

where n is the size of the vectors, and T is the numeric type (typically Float64).