1. Setting Up a Dynamics Model

TrajectoryOptimization relies on the interface defined by RobotDynamics.jl to define the forced dynamics required to solve the problem. Please refer to the documentation for more details on setting up and defining models. We present a simple example here.

Assume we want to find an optimal trajectory for the canonical cartpole system. We can either import an existing model defined in RobotZoo.jl or use RobotDynamics.jl to define our own. Defining our own model is pretty straight-forward:

using RobotDynamics
using ForwardDiff
using FiniteDiff
using StaticArrays

RobotDynamics.@autodiff struct Cartpole{T} <: RobotDynamics.ContinuousDynamics
    mc::T
    mp::T
    l::T
    g::T
end

Cartpole() = Cartpole(1.0, 0.2, 0.5, 9.81)

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

function RobotDynamics.dynamics(model::Cartpole, x, u)
    mc = model.mc   # mass of the cart in kg (10)
    mp = model.mp   # mass of the pole (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] ]  # SA[...] creates a StaticArray.
    qd = x[ SA[3,4] ]

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

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

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

function RobotDynamics.dynamics!(model::Cartpole, xdot, x, u)
    xstatic = SA[x[1], x[2], x[3], x[4]]
    ustatic = SA[u[1]]
    xdot .= RobotDynamics.dynamics(model, xstatic, ustatic)
    return nothing
end

with our dynamics model defined, we are ready to start setting up the optimization problem.