2. Evaluating the Dynamics

Overview

Once we have a model defined as detailed on the previous page, we can query both the continuous and discrete dynamics, and have access to some additional, useful methods.

Querying the Continuous Dynamics

We can evaluate the continuous dynamics using one of the following methods:

ẋ = dynamics(model, z)
ẋ = dynamics(model, x, u)
ẋ = dynamics(model, x, u, t)

where z is an AbstractKnotPoint, x is the n-dimensional state vector, u is the m-dimensional control vector, and t is the positive scalar independent variable, typically time. For best performance, x and u should be SVectors.

We can evaluate the continuous time dynamics Jacobian using the method

jacobian!(∇f, model, z)

where ∇f is an n × (n+m) matrix. Note that the Jacobian methods require an AbstractKnotPoint, since this eliminates unnecessary concatenation and subsequent memory allocations when using ForwardDiff.

The DynamicsJacobian type

While the dynamics Jacobian ∇f can be any AbstractMatrix, RobotDynamics provides the DynamicsJacobian type that has some convenient constructors and provides access to the individual partial derivatives:

RobotDynamics.DynamicsJacobian — Type
DynamicsJacobian{n,nm,T}

Custom n × (n+m) matrix specifying a dynamics Jacobian for a forced dynamical system with n states and m controls. The Jacobian is structured as [∂x ∂u] where x and u are the state and control vectors, respectively.

The DynamicsJacobianD provides access to the partial derivatives A = ∂x and B = ∂u via direct access D.A and D.B, returning a view into the underlying Matrix, or

RobotDynamics.get_A(D)
RobotDynamics.get_B(D)

which return an SMatrix. Note that this method should be used with caution for systems with large state and/or control dimensions.

Constructors

DynamicsJacobian(model::AbstractModel)
DynamicsJacobian(n::Int, m::Int)
DynamicsJacobian(D::StaticMatrix)

where D is a StaticMatrix of appropriate size. Since DynamicsJacobian implements the StaticMatrix interface, is also supports all the constructors and operations inherent to a StaticMatrix.

Querying Discrete Dynamics

The discrete dynamics can be evaluated using methods analogous to those used to evaluate the continuous dynamics, except we now need to specify the integration method and the time step.

x′ = discrete_dynamics(::Type{Q}, model, z)
x′ = discrete_dynamics(::Type{Q}, model, x, u, t, dt)

where Q is a QuadratureRule. See Discretization for more information on the integration methods defined in RobotDynamics.

When evaluating discrete dynamics, one can also use the propagate_dynamics method that updates the state of the next KnotPoint:

RobotDynamics.propagate_dynamics — Function
propagate_dynamics(::Type{Q}, model, z_, z)

Evaluate the discrete dynamics of model using integration method Q at knot point z, storing the result in the states of knot point z_.

Useful for propagating dynamics along a trajectory of knot points.

The discrete dynamics Jacobian is similarly evaluated using

discrete_jacobian!(::Type{Q}, ∇f, model, z)

If the integration method is not passed in as the first argument, the default integration method RK3 will be used.

Other Methods

All AbstractModels provide a few functions for generating state and control vectors directly from the model:

x,u = zeros(model)
x,u = rand(model)
x,u = fill(model, value)

The Base.size method is also overloaded as a shortcut for returning state_dim(model) and control_dim(model) as a tuple:

n,m = size(model)