Merge branch 'master' into toc

Author: bjack205

.github/workflows/TagBot.yml

@@ -1,6 +1,6 @@
 name = "TrajectoryOptimization"
 uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca"
-version = "0.1.2"
+version = "0.2.0"
 
 [deps]
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
@@ -11,3 +11,36 @@ RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 UnsafeArrays = "c4a57d5a-5b31-53a6-b365-19f8c011fbd6"
+
+[compat]
+BenchmarkTools = "0.4"
+Combinatorics = "1"
+CoordinateTransformations = "0.5,0.6"
+Distributions = "0.23"
+DocStringExtensions = "0.8"
+Documenter = "0.24"
+FFMPEG = "0.3"
+Formatting = "0.4"
+ForwardDiff = "0.10"
+Interpolations = "0.12"
+Ipopt = "0.6.0"
+IterTools = "1"
+MathOptInterface = "0.9"
+Parameters = "0.12"
+Quaternions = "0.4"
+RecipesBase = "1"
+Rotations = "0.13,1.0"
+StaticArrays = "0.12.3"
+StatsBase = "0.33"
+UnsafeArrays = "1"
+julia = "1.1.0"
+
+[extras]
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+Logging = "56ddb016-857b-54e1-b83d-db4d58db5568"
+Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
+SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
+Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+
+[targets]
+test = ["LinearAlgebra", "SparseArrays", "Random", "Test", "Logging", "ForwardDiff"]

Project.toml

@@ -16,59 +16,67 @@ A library of solvers for trajectory optimization problems written in Julia. Curr
 Direct Collocation (DIRCOL)
   * Interfaces to Nonlinear Programming solvers (e.g., [Ipopt](https://github.com/coin-or/Ipopt), [SNOPT](https://ccom.ucsd.edu/~optimizers/solvers/snopt/)) via [MathOptInterface](https://github.com/JuliaOpt/MathOptInterface.jl)
 
-All methods utilize Julia's extensive autodifferentiation capabilities via [ForwardDiff.jl](http://www.juliadiff.org/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](https://github.com/JuliaRobotics/RigidBodyDynamics.jl).
+All methods utilize Julia's extensive autodifferentiation capabilities via [ForwardDiff.jl](http://www.juliadiff.org/ForwardDiff.jl/) so that the user does not need to specify derivatives of dynamics, cost, or constraint functions.
 
 ## Installation
 To install TrajectoryOptimization.jl, run the following from the Julia REPL:
 ```julia
 Pkg.add("TrajectoryOptimization")
 ```
 
+# What's New
+`TrajectoryOptimization.jl` underwent significant changes between versions `v0.1` and `v0.2`. The new code is significantly faster (up to 100x faster). The core part of the ALTRO solver (everything except the projected newton phase) is completely allocation-free once the solver has been initialized. Most of the API has changed significantly. See the documentation for more information on the new API.
+
 ## Quick Start
-To run a simple example of a constrained 1D block move:
+To run a simple example of a constrained 1D block move (see script in `/examples/quickstart.jl`):
 ```julia
-using TrajectoryOptimization, LinearAlgebra
+using TrajectoryOptimization
+using StaticArrays
+using LinearAlgebra
+const TO = TrajectoryOptimization
 
-function dynamics!(ẋ,x,u) # inplace dynamics
-    ẋ[1] = x[2]
-    ẋ[2] = u[1]
+struct DoubleIntegrator{T} <: AbstractModel
+    mass::T
 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
+function TO.dynamics(model::DoubleIntegrator, x, u)
+    SA[x[2], u[1] / model.mass]
 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}())
+Base.size(::DoubleIntegrator) = 2,1
+
+# Model and discretization
+model = DoubleIntegrator(1.0)
+n,m = size(model)
+tf = 3.0  # sec
+N = 21    # number of knot points
+
+# Objective
+x0 = SA[0,0.]  # initial state
+xf = SA[1,0.]  # final state
+
+Q = Diagonal(@SVector ones(n))
+R = Diagonal(@SVector ones(m))
+obj = LQRObjective(Q, R, N*Q, xf, N)
+
+# Constraints
+cons = TO.ConstraintSet(n,m,N)
+add_constraint!(cons, GoalConstraint(xf), N:N)
+add_constraint!(cons, BoundConstraint(n,m, u_min=-10, u_max=10), 1:N-1)
+
+# Create and solve problem
+prob = Problem(model, obj, xf, tf, x0=x0, constraints=cons)
+solver = ALTROSolver(prob)
+cost(solver)           # initial cost
+solve!(solver)         # solve with ALTRO
+max_violation(solver)  # max constraint violation
+cost(solver)           # final cost
+iterations(solver)     # total number of iterations
+
+# Get the state and control trajectories
+X = states(solver)
+U = controls(solver)
 ```
 
 ## Examples
-Notebooks with more detailed examples can be found [here](https://github.com/RoboticExplorationLab/TrajectoryOptimization.jl/tree/master/examples), including all the examples from our [IROS 2019 paper](https://github.com/RoboticExplorationLab/TrajectoryOptimization.jl/tree/master/examples/IROS_2019).
-
-## Documentation
-Detailed documentation for getting started with the package can be found [here](https://roboticexplorationlab.github.io/TrajectoryOptimization.jl/dev/).
+Notebooks with more detailed examples can be found [here](https://github.com/RoboticExplorationLab/TrajectoryOptimization.jl/tree/master/examples), including all the examples from our [IROS 2019 paper](https://rexlab.stanford.edu/papers/altro-iros.pdf).

README.md

@@ -0,0 +1,46 @@
+using TrajectoryOptimization
+using StaticArrays
+using LinearAlgebra
+const TO = TrajectoryOptimization
+
+struct DoubleIntegrator{T} <: AbstractModel
+    mass::T
+end
+
+function TO.dynamics(model::DoubleIntegrator, x, u)
+    SA[x[2], u[1] / model.mass]
+end
+
+Base.size(::DoubleIntegrator) = 2,1
+
+# Model and discretization
+model = DoubleIntegrator(1.0)
+n,m = size(model)
+tf = 3.0  # sec
+N = 21    # number of knot points
+
+# Objective
+x0 = SA[0,0.]  # initial state
+xf = SA[1,0.]  # final state
+
+Q = Diagonal(@SVector ones(n))
+R = Diagonal(@SVector ones(m))
+obj = LQRObjective(Q, R, N*Q, xf, N)
+
+# Constraints
+cons = TO.ConstraintSet(n,m,N)
+add_constraint!(cons, GoalConstraint(xf), N:N)
+add_constraint!(cons, BoundConstraint(n,m, u_min=-10, u_max=10), 1:N-1)
+
+# Create and solve problem
+prob = Problem(model, obj, xf, tf, x0=x0, constraints=cons)
+solver = ALTROSolver(prob)
+cost(solver)           # initial cost
+solve!(solver)         # solve with ALTRO
+max_violation(solver)  # max constraint violation
+cost(solver)           # final cost
+iterations(solver)     # total number of iterations
+
+# Get the state and control trajectories
+X = states(solver)
+U = controls(solver)

examples/quickstart.jl

@@ -43,7 +43,7 @@ cartpole = Dynamics.Cartpole()
 # Quadrotor
 quad = Dynamics.Quadrotor2()
 @test size(quad) == (13,4)
-@test num_allocs(cartpole) == 0
+@test num_allocs(quad) == 0
 
 
 # Infeasible