Quickstart

This page provides a quick overview of the API to help you get started quickly. It also points to other places in the documentation to help you get more details.

Step 1: Define the Dynamics Model

Define the dynamics model. See documentation for RobotDynamics.jl for more details.

using TrajectoryOptimization
using RobotDynamics
using StaticArrays
using LinearAlgebra
using RobotDynamics
using Test
const TO = TrajectoryOptimization
const RD = RobotDynamics

## Model Definition
struct DoubleIntegrator{T} <: RD.ContinuousDynamics 
    mass::T
end

function RD.dynamics(model::DoubleIntegrator, x, u)
    mass = model.mass
    vx,vy = x[3], x[4]
    ax,ay = u[1] / mass, u[2] / mass
    SA[vx, vy, ax, ay]
end

RD.state_dim(::DoubleIntegrator) = 4
RD.control_dim(::DoubleIntegrator) = 2

Step 2: Define the Discretization

The next step is to instantiate our model, discretize it, if necessary, and define the number of knot points to use and time horizon.

model = DoubleIntegrator(1.0)
dmodel = RD.DiscretizedDynamics{RD.RK4}(model)
n,m = RD.dims(model)
tf = 3.0         # final time (sec)
N = 21           # number of knot points
dt = tf / (N-1)  # time step (sec)

Step 3: Define the Objective

We now define our objective. See Setting up an Objective Cost Function Inteface, and the Cost/Objective API.

# Objective
x0 = SA[0,0.,0,0]  # initial state
xf = SA[0,2.,0,0]  # final state

Q = Diagonal(@SVector ones(n))
R = Diagonal(@SVector ones(m))
Qf = Q*(N-1)
obj = LQRObjective(Q, R, Qf, xf, N)

Step 4: Define constraints

The next step is to define any constraints we want to add to our trajectory optimization problem. Here we define the following constraints:

• A constraint to get to the goal
• A single circular obstacle at (0,1.0) with radius 0.5
• Norm constraint on the controls: ||u||₂ <= 5.0
• Bounds the controls between -10 and 10

See Creating Constraints, Constraint Interface, and the Constraint API for more details.

cons = ConstraintList(n,m,N)
add_constraint!(cons, GoalConstraint(xf), N)
add_constraint!(cons, CircleConstraint(n, SA[0.0], SA[1.0], SA[0.5]), 2:N-1)
add_constraint!(cons, NormConstraint(n, m, 5.0, TO.SecondOrderCone(), :control), 1:N-1)
add_constraint!(cons, BoundConstraint(n,m, u_min=-10, u_max=10), 1:N-1)

Step 5: Create the Problem

The last step is to actually create the problem by passing all of the information we defined previously to the constructor. See Setting up a Problem and the Problem API for more details.

# Create problem
prob = Problem(model, obj, x0, tf, xf=xf, constraints=cons)

Step 6: Methods on Problems

The following code gives some examples of a few of the methods you can use on a Problem once it's created.

# Set initial controls
U0 = [randn(m) for k = 1:N]
initial_controls!(prob, U0)

# Set initial state
X0 = [zeros(n) for k = 1:N]
initial_states!(prob, X0)

# Evaluate the cost
J = cost(prob)
@test J ≈ mapreduce(+, 1:N) do k
    if k < N
        x,u = X0[k], U0[k]
        0.5 * (x - xf)'Q*(x - xf) + 0.5*u'R*u
    else
        x = X0[k]
        0.5 * (x - xf)'Qf*(x - xf)
    end
end

# Simulate the system forward
rollout!(prob)

# Extract states, controls, and times
X = states(prob)
U = controls(prob)
t = gettimes(prob)

Xrollout = [copy(x0) for k = 1:N]
for k = 1:N-1
    Xrollout[k+1] = RD.discrete_dynamics(
        get_model(prob, k), Xrollout[k], U0[k], dt*(k-1), dt
    )
end
@test Xrollout ≈ X 

# Convert to matrices
Xmat = hcat(Vector.(X)...)
Umat = hcat(Vector.(U)...)

# Extract out individual states and controls
x1 = states(prob, 2)
u1 = controls(prob, 1)

Step 7: Working with Constraints

This section provides a few more details and example on how to work with the constraints we defined previously.

The ConstraintList type supports indexing and iteration:

constraints = get_constraints(prob)
goalcon = constraints[1]
circlecon = constraints[2]
normcon = constraints[3]

You can use the zip method to iterate over the constraint and its assigned indices:

for (inds,con) in zip(constraints)
    @show inds
end

You can identify the type of constraint using the TrajectoryOptimization.sense function, which returns a ConstraintSense:

cones = map(TO.sense, constraints)

You can get the bounds of each constraint, which are useful when passing to interfaces like MathOptInterface that expect a vector of constraints with upper and lower bounds to encode equality/inequality constraints.

lower_bounds = vcat(Vector.(map(TO.lower_bound, constraints))...)
upper_bounds = vcat(Vector.(map(TO.upper_bound, constraints))...)
@test lower_bounds ≈ [zeros(n); fill(-Inf, 1); fill(-Inf, m+1); fill(-Inf, n); fill(-10, m)]
@test upper_bounds ≈ [zeros(n); fill(0.0, 1); fill(+Inf, m+1); fill(+Inf, n); fill(+10, m)]

You can use the is_bound function to check to see if the constraint can be represented as a simple bound constraint on the states or controls:

is_bound_constraint = map(TO.is_bound, constraints)

To evaluate constraints, you use the same interface from AbstractFunction defined in RobotDynamics:

# Evaluating a single constraint
# * Note that the `GoalConstraint` is only a function of the state, but
#   `NormConstraint` is a function of both the state and control
x,u = rand(model)  # generate some random states and controls
z = KnotPoint(x, u, 0.0, NaN)
c = zeros(RD.output_dim(goalcon))
RD.evaluate(goalcon, x)
RD.evaluate(goalcon, z)
RD.evaluate!(goalcon, c, x)
RD.evaluate!(goalcon, c, z)

c = zeros(RD.output_dim(normcon))
RD.evaluate(normcon, x, u)
RD.evaluate(normcon, z)
RD.evaluate!(normcon, c, x, u)
RD.evaluate!(normcon, c, z)

# Evaluating the Jacobian for a single constraint
# * Note that these methods require passing in a KnotPoint type
J = TO.gen_jacobian(goalcon)
c = zeros(RD.output_dim(goalcon))
RD.jacobian!(RD.StaticReturn(), RD.UserDefined(), goalcon, J, c, z)

J = TO.gen_jacobian(normcon)
c = zeros(RD.output_dim(normcon))
RD.jacobian!(RD.StaticReturn(), RD.UserDefined(), normcon, J, c, z)

# Evaluating constraint at multiple time steps 
Z = get_trajectory(prob)
inds = constraints.inds[1]
vals = [zeros(RD.output_dim(goalcon)) for i in inds] 
TO.evaluate_constraints!(RD.StaticReturn(), goalcon, vals, Z, inds)

inds = constraints.inds[2]
vals = [zeros(RD.output_dim(normcon)) for i in inds] 
TO.evaluate_constraints!(constraints.sigs[2], normcon, vals, Z, inds)

# Evaluating constraint Jacobians at multiple time steps
inds = constraints.inds[1]
jacs = [TO.gen_jacobian(goalcon) for i in inds] 
vals = [zeros(RD.output_dim(goalcon)) for i in inds] 
TO.constraint_jacobians!(
    TO.functionsignature(constraints, 1), TO.diffmethod(constraints, 1), 
    goalcon, jacs, vals, Z, inds
)

inds = constraints.inds[2]
jacs = [TO.gen_jacobian(normcon) for i in inds] 
vals = [zeros(RD.output_dim(normcon)) for i in inds] 
TO.constraint_jacobians!(
    TO.functionsignature(constraints, 2), TO.diffmethod(constraints, 2), 
    normcon, jacs, vals, Z, inds
)