RobotDynamics.jl

Welcome to RobotDynamics.jl! This package is dedicated to providing a convenient interface for defining the dynamics of forced dynamical systems, such as robots.

This package also provides many efficient methods for evaluating dynamics, their Jacobians, and their discrete-time versions for use in optimization packages such as TrajectoryOptimization.jl.

Getting Started

a = 1
b = 2
a + b

3

Defining a new continuous dynamics model

It is pretty straightforward to define a new dynamics model. We start by creating new struct that inherits from ContinuousDynamics. You can store any model parameters in the model struct:

using RobotDynamics
struct MyModel <: RobotDynamics.ContinuousDynamics
    mass::Float64
    spring_stiffness::Float64
end

We now need to specify the dimensions of our state and control vectors:

RobotDynamics.state_dim(::MyModel) = 2
RobotDynamics.control_dim(::MyModel) = 1

Now we're ready to define our dynamics functions. We can specify either in-place or out-of-place dynamics functions signatures. We start with the in-place definition:

function RobotDynamics.dynamics!(model::MyModel, xdot, x, u)
    # x is the state vector
    # u is the control vector
    p = x[1]  # position
    v = x[2]  # velocity
    pdot = v
    F = -model.spring_stiffness * p + u[1]  # force
    vdot = F / model.mass
    xdot[1] = pdot
    xdot[2] = vdot
    nothing
end

For small systems like this, it is often efficient to use out-of-place methods with StaticArrays. Our signature looks almost identical, except that it does not have a ! at the end, and does not have xdot as a argument:

using StaticArrays
function RobotDynamics.dynamics(model::MyModel, x, u)
    p = x[1]  # position
    v = x[2]  # velocity
    pdot = v
    F = -model.spring_stiffness * p + u[1]  # force
    vdot = F / model.mass
    return SA[pdot, vdot]  # shortcut to create an SVector
end

Calling the dynamics

Now that we've defined our dynamics, let's see how we can call it. This package provides the KnotPoint type that stores the state and control vector together with information about the time. Let's start by creating our model and some KnotPoint types:

mass = 1.0
stiffness = 0.5
model = MyModel(mass, stiffness)
n = RobotDynamics.state_dim(model)
m = RobotDynamics.control_dim(model)

# Create a KnotPoint using SVectors
# (should use this when calling out-of-place methods)
xs = @SVector randn(n)
us = @SVector randn(m)
t = 0.0   # current time
dt = NaN  # time step, not needed for continuous models
zs = KnotPoint(xs, us, t, dt)

# Create a KnotPoint using normal vectors
# (using 's' for static, 'd' for dynamic)
xd = Vector(xs)
ud = Vector(us)
zd = KnotPoint{n,m}(xd, ud, t, dt)

Now that we've defined some inputs, let's evaluate our continuous dynamics. We can use any of the methods below:

# Out-of-place methods
RobotDynamics.dynamics(model, zs)
RobotDynamics.dynamics(model, xs, us)
RobotDynamics.dynamics(model, xs, us, t)

# In-place methods
xdot = zeros(n)
RobotDynamics.dynamics!(model, xdot, zd)
RobotDynamics.dynamics!(model, xdot, xd, ud)
RobotDynamics.dynamics!(model, xdot, xd, ud, t)
xd

Querying the dynamics Jacobian

Writing down an analytical dynamics Jacobian can often be time-consuming and error-prone. RobotDynamics allows the user to specify, at run time, the method to be used to evaluate the dynamics Jacobians, as long as they are defined on your type. To automatically define methods that use ForwardDiff.jl or FiniteDiff.jl, you can use the @autodiff macro provided by this package. This modifies your model, adding new fields for the caches used by these methods, as well as the methods to evaluate them. For our example, all we have to do is add the macro before our model struct definition:

# NOTE: must include these packages when using `@autodiff`
using ForwardDiff
using FiniteDiff
RobotDynamics.@autodiff struct MyModel <: RobotDynamics.ContinuousDynamics
    mass::Float64
    spring_stiffness::Float64
end

After which we can use the following generic methods to evaluate our dynamics Jacobians:

J = zeros(n, n + m)
sig = RobotDynamics.StaticReturn()  # out-of-place signature
diff = RobotDynamics.ForwardAD()    # use forward AD differentiation
RobotDynamics.jacobian!(sig, diff, model, J, xdot, zs)

sig = RobotDynamics.InPlace()            # out-of-place signature
diff = RobotDynamics.FiniteDifference()  # use finite differences
RobotDynamics.jacobian!(sig, diff, model, J, xdot, zd)

These methods should always be called using a KnotPoint input, since it's more efficient to differentiation the dynamics treating the state and control as a single vector input into the dynamics, which is why the KnotPoint type stores them as a concatenated vector.

Discretizing our dynamics

We often need to discretize our continuous dynamics, either to simulate it or feed it into optimization frameworks. The DiscretizedDynamics type discretizes a ContinuousDynamics model, turning it into a DiscreteDynamics model by applying a QuadratureRule. To discretize our system using, e.g. Runge-Kutta 4, we create a new DiscretizedDynamics model:

rk4 = RobotDynamics.RK4(n, m)  # create the integrator
model_discrete = RobotDynamics.DiscretizedDynamics(model, rk4)

We can query our discrete dynamics using very similar methods to those we used before, except now we have to specify the time step.

# Specify a time step of 0.1 seconds
zs.dt = 0.1
zd.dt = 0.1

# Out-of-place methods
RobotDynamics.discrete_dynamics(model_discrete, zs)
RobotDynamics.discrete_dynamics(model_discrete, xs, us, t, dt)

# In-place methods
xn = zeros(n)
RobotDynamics.discrete_dynamics!(model_discrete, xn, zd)
RobotDynamics.discrete_dynamics!(model_discrete, xn, xd, ud, t, dt)

The Jacobians are called using the same methods used for the continuous dynamics.