A library of solvers for trajectory optimization problems written in Julia. Currently, the following methods are implemented with a common interface:
ALTRO (Augmented Lagrangian TRajectory Optimizer): A fast solver for constrained trajectory optimization problems formulated as MDPs that features:
- General nonlinear cost functions, including minimum time problems
- General nonlinear state and input constraints
- Infeasible state initialization
- Square-root methods for improved numerical conditioning
- Active-set projection method for solution polishing
Direct Collocation (DIRCOL)
- Interfaces to Nonlinear Programming solvers (e.g., Ipopt, SNOPT) via MathOptInterface
All methods utilize Julia's extensive autodifferentiation capabilities via ForwardDiff.jl so that the user does not need to specify derivatives of dynamics, cost, or constraint functions. Dynamics can be computed directly from a URDF file via RigidBodyDynamics.jl.
To install TrajectoryOptimization.jl, run the following from the Julia REPL:
Pkg.add("TrajectoryOptimization")To run a simple example of a constrained 1D block move:
using TrajectoryOptimization, LinearAlgebra
function dynamics!(ẋ,x,u) # inplace dynamics
ẋ[1] = x[2]
ẋ[2] = u[1]
end
n = 2 # number of states
m = 1 # number of controls
model = Model(dynamics!,n,m) # create model
model_d = rk3(model) # create discrete model w/ rk3 integration
x0 = [0.; 0.] # initial state
xf = [1.; 0.] # goal state
N = 21 # number of knot points
dt = 0.1 # time step
U0 = [0.01*rand(m) for k = 1:N-1]; # initial control trajectory
Q = 1.0*Diagonal(I,n)
Qf = 1.0*Diagonal(I,n)
R = 1.0e-1*Diagonal(I,m)
obj = LQRObjective(Q,R,Qf,xf,N) # objective
bnd = BoundConstraint(n,m,u_max=1.5, u_min=-1.5) # control limits
goal = goal_constraint(xf) # terminal constraint
constraints = Constraints(N) # define constraints at each time step
for k = 1:N-1
constraints[k] += bnd
end
constraints[N] += goal
prob = Problem(model_d, obj, constraints=constraints, x0=x0, xf=xf, N=N, dt=dt) # construct problem
initial_controls!(prob,U0) # initialize problem with controls
solver = solve!(prob, ALTROSolverOptions{Float64}())Notebooks with more detailed examples can be found here, including all the examples from our IROS 2019 paper.
Detailed documentation for getting started with the package can be found here.